12345qiupeng
/
gtsam
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

Merge pull request #280 from Alescontrela/feature/addPrior 

Add addPrior method to NonlinearFactorGraph and corresponding unit test.
release/4.3a0
Frank Dellaert 2020-04-17 16:48:06 -04:00 committed by GitHub
parent b6c5bceae9 30a109e38e
commit cce936f03a
No known key found for this signature in database
GPG Key ID: 4AEE18F83AFDEB23
97 changed files with 451 additions and 451 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

3
cmake/example_cmake_find_gtsam/main.cpp
View File

@ -33,7 +33,6 @@
// Here we will use Between factors for the relative motion described by odometry measurements.
// We will also use a Between Factor to encode the loop closure constraint
// Also, we will initialize the robot at the origin using a Prior factor.
#include <gtsam/slam/PriorFactor.h>
#include <gtsam/slam/BetweenFactor.h>
// When the factors are created, we will add them to a Factor Graph. As the factors we are using
@ -69,7 +68,7 @@ int main(int argc, char** argv) {
// 2a. Add a prior on the first pose, setting it to the origin
// A prior factor consists of a mean and a noise model (covariance matrix)
noiseModel::Diagonal::shared_ptr priorNoise = noiseModel::Diagonal::Sigmas(Vector3(0.3, 0.3, 0.1));
graph.emplace_shared<PriorFactor<Pose2> >(1, Pose2(0, 0, 0), priorNoise);
graph.addPrior(1, Pose2(0, 0, 0), priorNoise);
// For simplicity, we will use the same noise model for odometry and loop closures
noiseModel::Diagonal::shared_ptr model = noiseModel::Diagonal::Sigmas(Vector3(0.2, 0.2, 0.1));

2
doc/Code/OdometryExample.cpp
View File

@ -5,7 +5,7 @@ NonlinearFactorGraph graph;
Pose2 priorMean(0.0, 0.0, 0.0);
noiseModel::Diagonal::shared_ptr priorNoise =
noiseModel::Diagonal::Sigmas(Vector3(0.3, 0.3, 0.1));
graph.add(PriorFactor<Pose2>(1, priorMean, priorNoise));
graph.addPrior(1, priorMean, priorNoise);
// Add two odometry factors
Pose2 odometry(2.0, 0.0, 0.0);

2
doc/Code/Pose2SLAMExample.cpp
View File

@ -1,7 +1,7 @@
NonlinearFactorGraph graph;
noiseModel::Diagonal::shared_ptr priorNoise =
noiseModel::Diagonal::Sigmas(Vector3(0.3, 0.3, 0.1));
graph.add(PriorFactor<Pose2>(1, Pose2(0, 0, 0), priorNoise));
graph.addPrior(1, Pose2(0, 0, 0), priorNoise);
// Add odometry factors
noiseModel::Diagonal::shared_ptr model =

4
examples/ISAM2Example_SmartFactor.cpp
View File

@ -57,7 +57,7 @@ int main(int argc, char* argv[]) {
vector<Pose3> poses = {pose1, pose2, pose3, pose4, pose5};
// Add first pose
graph.emplace_shared<PriorFactor<Pose3>>(X(0), poses[0], noise);
graph.addPrior(X(0), poses[0], noise);
initialEstimate.insert(X(0), poses[0].compose(delta));
// Create smart factor with measurement from first pose only
@ -71,7 +71,7 @@ int main(int argc, char* argv[]) {
cout << "i = " << i << endl;
// Add prior on new pose
graph.emplace_shared<PriorFactor<Pose3>>(X(i), poses[i], noise);
graph.addPrior(X(i), poses[i], noise);
initialEstimate.insert(X(i), poses[i].compose(delta));
// "Simulate" measurement from this pose

10
examples/ISAM2_SmartFactorStereo_IMU.cpp
View File

@ -129,18 +129,16 @@ int main(int argc, char* argv[]) {
// Pose prior - at identity
auto priorPoseNoise = noiseModel::Diagonal::Sigmas(
(Vector(6) << Vector3::Constant(0.1), Vector3::Constant(0.1)).finished());
graph.emplace_shared<PriorFactor<Pose3>>(X(1), Pose3::identity(),
priorPoseNoise);
graph.addPrior(X(1), Pose3::identity(), priorPoseNoise);
initialEstimate.insert(X(0), Pose3::identity());
// Bias prior
graph.add(PriorFactor<imuBias::ConstantBias>(B(1), imu.priorImuBias,
imu.biasNoiseModel));
graph.addPrior(B(1), imu.priorImuBias,
imu.biasNoiseModel);
initialEstimate.insert(B(0), imu.priorImuBias);
// Velocity prior - assume stationary
graph.add(
PriorFactor<Vector3>(V(1), Vector3(0, 0, 0), imu.velocityNoiseModel));
graph.addPrior(V(1), Vector3(0, 0, 0), imu.velocityNoiseModel);
initialEstimate.insert(V(0), Vector3(0, 0, 0));
int lastFrame = 1;

10
examples/ImuFactorExample2.cpp
View File

@ -7,7 +7,6 @@
#include <gtsam/navigation/Scenario.h>
#include <gtsam/nonlinear/ISAM2.h>
#include <gtsam/slam/BetweenFactor.h>
#include <gtsam/slam/PriorFactor.h>
#include <vector>
@ -61,21 +60,18 @@ int main(int argc, char* argv[]) {
// 0.1 rad std on roll, pitch, yaw, 30cm std on x,y,z.
auto noise = noiseModel::Diagonal::Sigmas(
(Vector(6) << Vector3::Constant(0.1), Vector3::Constant(0.3)).finished());
newgraph.push_back(PriorFactor<Pose3>(X(0), pose_0, noise));
newgraph.addPrior(X(0), pose_0, noise);
// Add imu priors
Key biasKey = Symbol('b', 0);
auto biasnoise = noiseModel::Diagonal::Sigmas(Vector6::Constant(0.1));
PriorFactor<imuBias::ConstantBias> biasprior(biasKey, imuBias::ConstantBias(),
biasnoise);
newgraph.push_back(biasprior);
newgraph.addPrior(biasKey, imuBias::ConstantBias(), biasnoise);
initialEstimate.insert(biasKey, imuBias::ConstantBias());
auto velnoise = noiseModel::Diagonal::Sigmas(Vector3(0.1, 0.1, 0.1));
Vector n_velocity(3);
n_velocity << 0, angular_velocity * radius, 0;
PriorFactor<Vector> velprior(V(0), n_velocity, velnoise);
newgraph.push_back(velprior);
newgraph.addPrior(V(0), n_velocity, velnoise);
initialEstimate.insert(V(0), n_velocity);

7
examples/ImuFactorsExample.cpp
View File

@ -41,7 +41,6 @@
#include <gtsam/navigation/ImuFactor.h>
#include <gtsam/slam/dataset.h>
#include <gtsam/slam/BetweenFactor.h>
#include <gtsam/slam/PriorFactor.h>
#include <gtsam/nonlinear/LevenbergMarquardtOptimizer.h>
#include <gtsam/nonlinear/NonlinearFactorGraph.h>
#include <gtsam/inference/Symbol.h>
@ -129,9 +128,9 @@ int main(int argc, char* argv[])
// Add all prior factors (pose, velocity, bias) to the graph.
NonlinearFactorGraph *graph = new NonlinearFactorGraph();
graph->add(PriorFactor<Pose3>(X(correction_count), prior_pose, pose_noise_model));
graph->add(PriorFactor<Vector3>(V(correction_count), prior_velocity,velocity_noise_model));
graph->add(PriorFactor<imuBias::ConstantBias>(B(correction_count), prior_imu_bias,bias_noise_model));
graph->addPrior(X(correction_count), prior_pose, pose_noise_model);
graph->addPrior(V(correction_count), prior_velocity,velocity_noise_model);
graph->addPrior(B(correction_count), prior_imu_bias,bias_noise_model);
// We use the sensor specs to build the noise model for the IMU factor.
double accel_noise_sigma = 0.0003924;

1
examples/InverseKinematicsExampleExpressions.cpp
View File

@ -22,7 +22,6 @@
#include <gtsam/nonlinear/Marginals.h>
#include <gtsam/nonlinear/expressions.h>
#include <gtsam/slam/BetweenFactor.h>
#include <gtsam/slam/PriorFactor.h>
#include <gtsam/slam/expressions.h>
#include <cmath>

3
examples/METISOrderingExample.cpp
View File

@ -22,7 +22,6 @@
*/
#include <gtsam/geometry/Pose2.h>
#include <gtsam/slam/PriorFactor.h>
#include <gtsam/slam/BetweenFactor.h>
#include <gtsam/nonlinear/NonlinearFactorGraph.h>
#include <gtsam/nonlinear/LevenbergMarquardtOptimizer.h>
@ -37,7 +36,7 @@ int main(int argc, char** argv) {
Pose2 priorMean(0.0, 0.0, 0.0); // prior at origin
noiseModel::Diagonal::shared_ptr priorNoise = noiseModel::Diagonal::Sigmas(Vector3(0.3, 0.3, 0.1));
graph.emplace_shared<PriorFactor<Pose2> >(1, priorMean, priorNoise);
graph.addPrior(1, priorMean, priorNoise);
Pose2 odometry(2.0, 0.0, 0.0);
noiseModel::Diagonal::shared_ptr odometryNoise = noiseModel::Diagonal::Sigmas(Vector3(0.2, 0.2, 0.1));

3
examples/OdometryExample.cpp
View File

@ -29,7 +29,6 @@
// have been provided with the library for solving robotics/SLAM/Bundle Adjustment problems.
// Here we will use Between factors for the relative motion described by odometry measurements.
// Also, we will initialize the robot at the origin using a Prior factor.
#include <gtsam/slam/PriorFactor.h>
#include <gtsam/slam/BetweenFactor.h>
// When the factors are created, we will add them to a Factor Graph. As the factors we are using
@ -65,7 +64,7 @@ int main(int argc, char** argv) {
// A prior factor consists of a mean and a noise model (covariance matrix)
Pose2 priorMean(0.0, 0.0, 0.0); // prior at origin
noiseModel::Diagonal::shared_ptr priorNoise = noiseModel::Diagonal::Sigmas(Vector3(0.3, 0.3, 0.1));
graph.emplace_shared<PriorFactor<Pose2> >(1, priorMean, priorNoise);
graph.addPrior(1, priorMean, priorNoise);
// Add odometry factors
Pose2 odometry(2.0, 0.0, 0.0);

3
examples/PlanarSLAMExample.cpp
View File

@ -40,7 +40,6 @@
// Here we will use a RangeBearing factor for the range-bearing measurements to identified
// landmarks, and Between factors for the relative motion described by odometry measurements.
// Also, we will initialize the robot at the origin using a Prior factor.
#include <gtsam/slam/PriorFactor.h>
#include <gtsam/slam/BetweenFactor.h>
#include <gtsam/sam/BearingRangeFactor.h>
@ -81,7 +80,7 @@ int main(int argc, char** argv) {
// Add a prior on pose x1 at the origin. A prior factor consists of a mean and a noise model (covariance matrix)
Pose2 prior(0.0, 0.0, 0.0); // prior mean is at origin
noiseModel::Diagonal::shared_ptr priorNoise = noiseModel::Diagonal::Sigmas(Vector3(0.3, 0.3, 0.1)); // 30cm std on x,y, 0.1 rad on theta
graph.emplace_shared<PriorFactor<Pose2> >(x1, prior, priorNoise); // add directly to graph
graph.addPrior(x1, prior, priorNoise); // add directly to graph
// Add two odometry factors
Pose2 odometry(2.0, 0.0, 0.0); // create a measurement for both factors (the same in this case)

3
examples/Pose2SLAMExample.cpp
View File

@ -36,7 +36,6 @@
// Here we will use Between factors for the relative motion described by odometry measurements.
// We will also use a Between Factor to encode the loop closure constraint
// Also, we will initialize the robot at the origin using a Prior factor.
#include <gtsam/slam/PriorFactor.h>
#include <gtsam/slam/BetweenFactor.h>
// When the factors are created, we will add them to a Factor Graph. As the factors we are using
@ -72,7 +71,7 @@ int main(int argc, char** argv) {
// 2a. Add a prior on the first pose, setting it to the origin
// A prior factor consists of a mean and a noise model (covariance matrix)
noiseModel::Diagonal::shared_ptr priorNoise = noiseModel::Diagonal::Sigmas(Vector3(0.3, 0.3, 0.1));
graph.emplace_shared<PriorFactor<Pose2> >(1, Pose2(0, 0, 0), priorNoise);
graph.addPrior(1, Pose2(0, 0, 0), priorNoise);
// For simplicity, we will use the same noise model for odometry and loop closures
noiseModel::Diagonal::shared_ptr model = noiseModel::Diagonal::Sigmas(Vector3(0.2, 0.2, 0.1));

1
examples/Pose2SLAMExampleExpressions.cpp
View File

@ -21,7 +21,6 @@
#include <gtsam/nonlinear/ExpressionFactorGraph.h>
// For an explanation of headers below, please see Pose2SLAMExample.cpp
#include <gtsam/slam/PriorFactor.h>
#include <gtsam/slam/BetweenFactor.h>
#include <gtsam/geometry/Pose2.h>
#include <gtsam/nonlinear/GaussNewtonOptimizer.h>

3
examples/Pose2SLAMExample_g2o.cpp
View File

@ -19,7 +19,6 @@
*/
#include <gtsam/slam/dataset.h>
#include <gtsam/slam/PriorFactor.h>
#include <gtsam/geometry/Pose2.h>
#include <gtsam/nonlinear/GaussNewtonOptimizer.h>
#include <fstream>
@ -65,7 +64,7 @@ int main(const int argc, const char *argv[]) {
// Add prior on the pose having index (key) = 0
noiseModel::Diagonal::shared_ptr priorModel = //
noiseModel::Diagonal::Variances(Vector3(1e-6, 1e-6, 1e-8));
graph->add(PriorFactor<Pose2>(0, Pose2(), priorModel));
graph->addPrior(0, Pose2(), priorModel);
std::cout << "Adding prior on pose 0 " << std::endl;
GaussNewtonParams params;

3
examples/Pose2SLAMExample_graph.cpp
View File

@ -17,7 +17,6 @@
*/
// For an explanation of headers below, please see Pose2SLAMExample.cpp
#include <gtsam/slam/PriorFactor.h>
#include <gtsam/slam/BetweenFactor.h>
#include <gtsam/geometry/Pose2.h>
#include <gtsam/nonlinear/LevenbergMarquardtOptimizer.h>
@ -43,7 +42,7 @@ int main (int argc, char** argv) {
// Add a Gaussian prior on first poses
Pose2 priorMean(0.0, 0.0, 0.0); // prior at origin
SharedDiagonal priorNoise = noiseModel::Diagonal::Sigmas(Vector3(0.01, 0.01, 0.01));
graph->push_back(PriorFactor<Pose2>(0, priorMean, priorNoise));
graph -> addPrior(0, priorMean, priorNoise);
// Single Step Optimization using Levenberg-Marquardt
Values result = LevenbergMarquardtOptimizer(*graph, *initial).optimize();

3
examples/Pose2SLAMExample_graphviz.cpp
View File

@ -17,7 +17,6 @@
*/
// For an explanation of headers below, please see Pose2SLAMExample.cpp
#include <gtsam/slam/PriorFactor.h>
#include <gtsam/slam/BetweenFactor.h>
#include <gtsam/geometry/Pose2.h>
#include <gtsam/nonlinear/LevenbergMarquardtOptimizer.h>
@ -33,7 +32,7 @@ int main (int argc, char** argv) {
// 2a. Add a prior on the first pose, setting it to the origin
noiseModel::Diagonal::shared_ptr priorNoise = noiseModel::Diagonal::Sigmas(Vector3(0.3, 0.3, 0.1));
graph.emplace_shared<PriorFactor<Pose2> >(1, Pose2(0, 0, 0), priorNoise);
graph.addPrior(1, Pose2(0, 0, 0), priorNoise);
// For simplicity, we will use the same noise model for odometry and loop closures
noiseModel::Diagonal::shared_ptr model = noiseModel::Diagonal::Sigmas(Vector3(0.2, 0.2, 0.1));

3
examples/Pose2SLAMExample_lago.cpp
View File

@ -21,7 +21,6 @@
#include <gtsam/slam/lago.h>
#include <gtsam/slam/dataset.h>
#include <gtsam/slam/PriorFactor.h>
#include <gtsam/geometry/Pose2.h>
#include <fstream>
@ -44,7 +43,7 @@ int main(const int argc, const char *argv[]) {
// Add prior on the pose having index (key) = 0
noiseModel::Diagonal::shared_ptr priorModel = //
noiseModel::Diagonal::Variances(Vector3(1e-6, 1e-6, 1e-8));
graph->add(PriorFactor<Pose2>(0, Pose2(), priorModel));
graph->addPrior(0, Pose2(), priorModel);
graph->print();
std::cout << "Computing LAGO estimate" << std::endl;

3
examples/Pose2SLAMStressTest.cpp
View File

@ -19,7 +19,6 @@
#include <gtsam/nonlinear/NonlinearFactorGraph.h>
#include <gtsam/nonlinear/Values.h>
#include <gtsam/slam/BetweenFactor.h>
#include <gtsam/slam/PriorFactor.h>
#include <gtsam/slam/StereoFactor.h>
#include <random>
@ -48,7 +47,7 @@ void testGtsam(int numberNodes) {
Pose3 first = Pose3(first_M);
NonlinearFactorGraph graph;
graph.add(PriorFactor<Pose3>(0, first, priorModel));
graph.addPrior(0, first, priorModel);
// vo noise model
auto VOCovarianceModel = noiseModel::Isotropic::Variance(6, 1e-3);

3
examples/Pose2SLAMwSPCG.cpp
View File

@ -17,7 +17,6 @@
*/
// For an explanation of headers below, please see Pose2SLAMExample.cpp
#include <gtsam/slam/PriorFactor.h>
#include <gtsam/slam/BetweenFactor.h>
#include <gtsam/geometry/Pose2.h>
#include <gtsam/nonlinear/LevenbergMarquardtOptimizer.h>
@ -36,7 +35,7 @@ int main(int argc, char** argv) {
// 2a. Add a prior on the first pose, setting it to the origin
Pose2 prior(0.0, 0.0, 0.0); // prior at origin
noiseModel::Diagonal::shared_ptr priorNoise = noiseModel::Diagonal::Sigmas(Vector3(0.3, 0.3, 0.1));
graph.emplace_shared<PriorFactor<Pose2> >(1, prior, priorNoise);
graph.addPrior(1, prior, priorNoise);
// 2b. Add odometry factors
noiseModel::Diagonal::shared_ptr odometryNoise = noiseModel::Diagonal::Sigmas(Vector3(0.2, 0.2, 0.1));

3
examples/Pose3Localization.cpp
View File

@ -19,7 +19,6 @@
#include <gtsam/slam/dataset.h>
#include <gtsam/slam/BetweenFactor.h>
#include <gtsam/slam/PriorFactor.h>
#include <gtsam/nonlinear/GaussNewtonOptimizer.h>
#include <gtsam/nonlinear/Marginals.h>
#include <fstream>
@ -48,7 +47,7 @@ int main(const int argc, const char *argv[]) {
for(const Values::ConstKeyValuePair& key_value: *initial) {
std::cout << "Adding prior to g2o file " << std::endl;
firstKey = key_value.key;
graph->add(PriorFactor<Pose3>(firstKey, Pose3(), priorModel));
graph->addPrior(firstKey, Pose3(), priorModel);
break;
}

1
examples/Pose3SLAMExample_changeKeys.cpp
View File

@ -19,7 +19,6 @@
#include <gtsam/slam/dataset.h>
#include <gtsam/slam/BetweenFactor.h>
#include <gtsam/slam/PriorFactor.h>
#include <fstream>
using namespace std;

3
examples/Pose3SLAMExample_g2o.cpp
View File

@ -19,7 +19,6 @@
#include <gtsam/slam/dataset.h>
#include <gtsam/slam/BetweenFactor.h>
#include <gtsam/slam/PriorFactor.h>
#include <gtsam/nonlinear/GaussNewtonOptimizer.h>
#include <fstream>
@ -47,7 +46,7 @@ int main(const int argc, const char *argv[]) {
for(const Values::ConstKeyValuePair& key_value: *initial) {
std::cout << "Adding prior to g2o file " << std::endl;
firstKey = key_value.key;
graph->add(PriorFactor<Pose3>(firstKey, Pose3(), priorModel));
graph->addPrior(firstKey, Pose3(), priorModel);
break;
}

3
examples/Pose3SLAMExample_initializePose3Chordal.cpp
View File

@ -20,7 +20,6 @@
#include <gtsam/slam/InitializePose3.h>
#include <gtsam/slam/dataset.h>
#include <gtsam/slam/BetweenFactor.h>
#include <gtsam/slam/PriorFactor.h>
#include <fstream>
using namespace std;
@ -47,7 +46,7 @@ int main(const int argc, const char *argv[]) {
for(const Values::ConstKeyValuePair& key_value: *initial) {
std::cout << "Adding prior to g2o file " << std::endl;
firstKey = key_value.key;
graph->add(PriorFactor<Pose3>(firstKey, Pose3(), priorModel));
graph->addPrior(firstKey, Pose3(), priorModel);
break;
}

3
examples/Pose3SLAMExample_initializePose3Gradient.cpp
View File

@ -20,7 +20,6 @@
#include <gtsam/slam/InitializePose3.h>
#include <gtsam/slam/dataset.h>
#include <gtsam/slam/BetweenFactor.h>
#include <gtsam/slam/PriorFactor.h>
#include <fstream>
using namespace std;
@ -47,7 +46,7 @@ int main(const int argc, const char *argv[]) {
for(const Values::ConstKeyValuePair& key_value: *initial) {
std::cout << "Adding prior to g2o file " << std::endl;
firstKey = key_value.key;
graph->add(PriorFactor<Pose3>(firstKey, Pose3(), priorModel));
graph->addPrior(firstKey, Pose3(), priorModel);
break;
}

3
examples/RangeISAMExample_plaza2.cpp
View File

@ -37,7 +37,6 @@
// In GTSAM, measurement functions are represented as 'factors'. Several common factors
// have been provided with the library for solving robotics SLAM problems.
#include <gtsam/slam/PriorFactor.h>
#include <gtsam/slam/BetweenFactor.h>
#include <gtsam/sam/RangeFactor.h>
#include <gtsam/slam/dataset.h>
@ -124,7 +123,7 @@ int main (int argc, char** argv) {
Pose2 pose0 = Pose2(-34.2086489999201, 45.3007639991120,
M_PI - 2.02108900000000);
NonlinearFactorGraph newFactors;
newFactors.push_back(PriorFactor<Pose2>(0, pose0, priorNoise));
newFactors.addPrior(0, pose0, priorNoise);
Values initial;
initial.insert(0, pose0);

5
examples/SFMExample.cpp
View File

@ -30,7 +30,6 @@
// have been provided with the library for solving robotics/SLAM/Bundle Adjustment problems.
// Here we will use Projection factors to model the camera's landmark observations.
// Also, we will initialize the robot at some location using a Prior factor.
#include <gtsam/slam/PriorFactor.h>
#include <gtsam/slam/ProjectionFactor.h>
// When the factors are created, we will add them to a Factor Graph. As the factors we are using
@ -75,7 +74,7 @@ int main(int argc, char* argv[]) {
// Add a prior on pose x1. This indirectly specifies where the origin is.
noiseModel::Diagonal::shared_ptr poseNoise = noiseModel::Diagonal::Sigmas((Vector(6) << Vector3::Constant(0.1), Vector3::Constant(0.3)).finished()); // 30cm std on x,y,z 0.1 rad on roll,pitch,yaw
graph.emplace_shared<PriorFactor<Pose3> >(Symbol('x', 0), poses[0], poseNoise); // add directly to graph
graph.addPrior(Symbol('x', 0), poses[0], poseNoise); // add directly to graph
// Simulated measurements from each camera pose, adding them to the factor graph
for (size_t i = 0; i < poses.size(); ++i) {
@ -90,7 +89,7 @@ int main(int argc, char* argv[]) {
// Here we add a prior on the position of the first landmark. This fixes the scale by indicating the distance
// between the first camera and the first landmark. All other landmark positions are interpreted using this scale.
noiseModel::Isotropic::shared_ptr pointNoise = noiseModel::Isotropic::Sigma(3, 0.1);
graph.emplace_shared<PriorFactor<Point3> >(Symbol('l', 0), points[0], pointNoise); // add directly to graph
graph.addPrior(Symbol('l', 0), points[0], pointNoise); // add directly to graph
graph.print("Factor Graph:\n");
// Create the data structure to hold the initial estimate to the solution

4
examples/SFMExample_SmartFactor.cpp
View File

@ -82,13 +82,13 @@ int main(int argc, char* argv[]) {
// 30cm std on x,y,z 0.1 rad on roll,pitch,yaw
noiseModel::Diagonal::shared_ptr noise = noiseModel::Diagonal::Sigmas(
(Vector(6) << Vector3::Constant(0.1), Vector3::Constant(0.3)).finished());
graph.emplace_shared<PriorFactor<Pose3> >(0, poses[0], noise);
graph.addPrior(0, poses[0], noise);
// Because the structure-from-motion problem has a scale ambiguity, the problem is
// still under-constrained. Here we add a prior on the second pose x1, so this will
// fix the scale by indicating the distance between x0 and x1.
// Because these two are fixed, the rest of the poses will be also be fixed.
graph.emplace_shared<PriorFactor<Pose3> >(1, poses[1], noise); // add directly to graph
graph.addPrior(1, poses[1], noise); // add directly to graph
graph.print("Factor Graph:\n");

4
examples/SFMExample_SmartFactorPCG.cpp
View File

@ -74,10 +74,10 @@ int main(int argc, char* argv[]) {
// 30cm std on x,y,z 0.1 rad on roll,pitch,yaw
noiseModel::Diagonal::shared_ptr noise = noiseModel::Diagonal::Sigmas(
(Vector(6) << Vector3::Constant(0.1), Vector3::Constant(0.3)).finished());
graph.emplace_shared<PriorFactor<Pose3> >(0, poses[0], noise);
graph.addPrior(0, poses[0], noise);
// Fix the scale ambiguity by adding a prior
graph.emplace_shared<PriorFactor<Pose3> >(1, poses[0], noise);
graph.addPrior(1, poses[0], noise);
// Create the initial estimate to the solution
Values initialEstimate;

5
examples/SFMExample_bal.cpp
View File

@ -19,7 +19,6 @@
#include <gtsam/inference/Symbol.h>
#include <gtsam/nonlinear/NonlinearFactorGraph.h>
#include <gtsam/nonlinear/LevenbergMarquardtOptimizer.h>
#include <gtsam/slam/PriorFactor.h>
#include <gtsam/slam/GeneralSFMFactor.h>
#include <gtsam/slam/dataset.h> // for loading BAL datasets !
#include <vector>
@ -66,8 +65,8 @@ int main (int argc, char* argv[]) {
// Add a prior on pose x1. This indirectly specifies where the origin is.
// and a prior on the position of the first landmark to fix the scale
graph.emplace_shared<PriorFactor<SfmCamera> >(C(0), mydata.cameras[0], noiseModel::Isotropic::Sigma(9, 0.1));
graph.emplace_shared<PriorFactor<Point3> > (P(0), mydata.tracks[0].p, noiseModel::Isotropic::Sigma(3, 0.1));
graph.addPrior(C(0), mydata.cameras[0], noiseModel::Isotropic::Sigma(9, 0.1));
graph.addPrior(P(0), mydata.tracks[0].p, noiseModel::Isotropic::Sigma(3, 0.1));
// Create initial estimate
Values initial;

5
examples/SFMExample_bal_COLAMD_METIS.cpp
View File

@ -21,7 +21,6 @@
#include <gtsam/inference/Ordering.h>
#include <gtsam/nonlinear/NonlinearFactorGraph.h>
#include <gtsam/nonlinear/LevenbergMarquardtOptimizer.h>
#include <gtsam/slam/PriorFactor.h>
#include <gtsam/slam/GeneralSFMFactor.h>
#include <gtsam/slam/dataset.h> // for loading BAL datasets !
@ -71,8 +70,8 @@ int main (int argc, char* argv[]) {
// Add a prior on pose x1. This indirectly specifies where the origin is.
// and a prior on the position of the first landmark to fix the scale
graph.emplace_shared<PriorFactor<SfmCamera> >(C(0), mydata.cameras[0], noiseModel::Isotropic::Sigma(9, 0.1));
graph.emplace_shared<PriorFactor<Point3> >(P(0), mydata.tracks[0].p, noiseModel::Isotropic::Sigma(3, 0.1));
graph.addPrior(C(0), mydata.cameras[0], noiseModel::Isotropic::Sigma(9, 0.1));
graph.addPrior(P(0), mydata.tracks[0].p, noiseModel::Isotropic::Sigma(3, 0.1));
// Create initial estimate
Values initial;

7
examples/SelfCalibrationExample.cpp
View File

@ -33,7 +33,6 @@
#include <gtsam/nonlinear/Values.h>
// SFM-specific factors
#include <gtsam/slam/PriorFactor.h>
#include <gtsam/slam/GeneralSFMFactor.h> // does calibration !
// Standard headers
@ -54,7 +53,7 @@ int main(int argc, char* argv[]) {
// Add a prior on pose x1.
noiseModel::Diagonal::shared_ptr poseNoise = noiseModel::Diagonal::Sigmas((Vector(6) << Vector3::Constant(0.1), Vector3::Constant(0.3)).finished()); // 30cm std on x,y,z 0.1 rad on roll,pitch,yaw
graph.emplace_shared<PriorFactor<Pose3> >(Symbol('x', 0), poses[0], poseNoise);
graph.addPrior(Symbol('x', 0), poses[0], poseNoise);
// Simulated measurements from each camera pose, adding them to the factor graph
Cal3_S2 K(50.0, 50.0, 0.0, 50.0, 50.0);
@ -71,11 +70,11 @@ int main(int argc, char* argv[]) {
// Add a prior on the position of the first landmark.
noiseModel::Isotropic::shared_ptr pointNoise = noiseModel::Isotropic::Sigma(3, 0.1);
graph.emplace_shared<PriorFactor<Point3> >(Symbol('l', 0), points[0], pointNoise); // add directly to graph
graph.addPrior(Symbol('l', 0), points[0], pointNoise); // add directly to graph
// Add a prior on the calibration.
noiseModel::Diagonal::shared_ptr calNoise = noiseModel::Diagonal::Sigmas((Vector(5) << 500, 500, 0.1, 100, 100).finished());
graph.emplace_shared<PriorFactor<Cal3_S2> >(Symbol('K', 0), K, calNoise);
graph.addPrior(Symbol('K', 0), K, calNoise);
// Create the initial estimate to the solution
// now including an estimate on the camera calibration parameters

7
examples/SimpleRotation.cpp
View File

@ -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/slam/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");
/**

5
examples/SolverComparer.cpp
View File

@ -34,7 +34,6 @@
#include <gtsam/slam/BetweenFactor.h>
#include <gtsam/sam/BearingRangeFactor.h>
#include <gtsam/slam/dataset.h>
#include <gtsam/slam/PriorFactor.h>
#include <gtsam/geometry/Pose2.h>
#include <gtsam/nonlinear/ISAM2.h>
#include <gtsam/nonlinear/GaussNewtonOptimizer.h>
@ -281,7 +280,7 @@ void runIncremental()
NonlinearFactorGraph newFactors;
Values newVariables;
newFactors.push_back(boost::make_shared<PriorFactor<Pose> >(firstPose, Pose(), noiseModel::Unit::Create(3)));
newFactors.addPrior(firstPose, Pose(), noiseModel::Unit::Create(3));
newVariables.insert(firstPose, Pose());
isam2.update(newFactors, newVariables);
@ -464,7 +463,7 @@ void runBatch()
cout << "Creating batch optimizer..." << endl;
NonlinearFactorGraph measurements = datasetMeasurements;
measurements.push_back(boost::make_shared<PriorFactor<Pose> >(0, Pose(), noiseModel::Unit::Create(3)));
measurements.addPrior(0, Pose(), noiseModel::Unit::Create(3));
gttic_(Create_optimizer);
GaussNewtonParams params;

7
examples/VisualISAM2Example.cpp
View File

@ -47,7 +47,6 @@
// Adjustment problems. Here we will use Projection factors to model the
// camera's landmark observations. Also, we will initialize the robot at some
// location using a Prior factor.
#include <gtsam/slam/PriorFactor.h>
#include <gtsam/slam/ProjectionFactor.h>
#include <vector>
@ -110,13 +109,11 @@ int main(int argc, char* argv[]) {
static auto kPosePrior = noiseModel::Diagonal::Sigmas(
(Vector(6) << Vector3::Constant(0.1), Vector3::Constant(0.3))
.finished());
graph.emplace_shared<PriorFactor<Pose3> >(Symbol('x', 0), poses[0],
kPosePrior);
graph.addPrior(Symbol('x', 0), poses[0], kPosePrior);
// Add a prior on landmark l0
static auto kPointPrior = noiseModel::Isotropic::Sigma(3, 0.1);
graph.emplace_shared<PriorFactor<Point3> >(Symbol('l', 0), points[0],
kPointPrior);
graph.addPrior(Symbol('l', 0), points[0], kPointPrior);
// Add initial guesses to all observed landmarks
// Intentionally initialize the variables off from the ground truth

5
examples/VisualISAMExample.cpp
View File

@ -38,7 +38,6 @@
// have been provided with the library for solving robotics/SLAM/Bundle Adjustment problems.
// Here we will use Projection factors to model the camera's landmark observations.
// Also, we will initialize the robot at some location using a Prior factor.
#include <gtsam/slam/PriorFactor.h>
#include <gtsam/slam/ProjectionFactor.h>
// We want to use iSAM to solve the structure-from-motion problem incrementally, so
@ -108,12 +107,12 @@ int main(int argc, char* argv[]) {
// Add a prior on pose x0, with 30cm std on x,y,z 0.1 rad on roll,pitch,yaw
noiseModel::Diagonal::shared_ptr poseNoise = noiseModel::Diagonal::Sigmas(
(Vector(6) << Vector3::Constant(0.1), Vector3::Constant(0.3)).finished());
graph.emplace_shared<PriorFactor<Pose3> >(Symbol('x', 0), poses[0], poseNoise);
graph.addPrior(Symbol('x', 0), poses[0], poseNoise);
// Add a prior on landmark l0
noiseModel::Isotropic::shared_ptr pointNoise =
noiseModel::Isotropic::Sigma(3, 0.1);
graph.emplace_shared<PriorFactor<Point3> >(Symbol('l', 0), points[0], pointNoise);
graph.addPrior(Symbol('l', 0), points[0], pointNoise);
// Add initial guesses to all observed landmarks
Point3 noise(-0.25, 0.20, 0.15);

2
examples/easyPoint2KalmanFilter.cpp
View File

@ -23,7 +23,7 @@
#include <gtsam/nonlinear/ExtendedKalmanFilter.h>
#include <gtsam/inference/Symbol.h>
#include <gtsam/slam/PriorFactor.h>
#include <gtsam/nonlinear/PriorFactor.h>
#include <gtsam/slam/BetweenFactor.h>
#include <gtsam/geometry/Point2.h>

2
examples/elaboratePoint2KalmanFilter.cpp
View File

@ -20,7 +20,7 @@
* @author Stephen Williams
*/
#include <gtsam/slam/PriorFactor.h>
#include <gtsam/nonlinear/PriorFactor.h>
#include <gtsam/slam/BetweenFactor.h>
//#include <gtsam/nonlinear/Ordering.h>
#include <gtsam/inference/Symbol.h>

2
gtsam.h
View File

@ -2494,7 +2494,7 @@ class NonlinearISAM {
#include <gtsam/geometry/CalibratedCamera.h>
#include <gtsam/geometry/StereoPoint2.h>
#include <gtsam/slam/PriorFactor.h>
#include <gtsam/nonlinear/PriorFactor.h>
template<T = {Vector, gtsam::Point2, gtsam::StereoPoint2, gtsam::Point3, gtsam::Rot2, gtsam::SO3, gtsam::SO4, gtsam::Rot3, gtsam::Pose2, gtsam::Pose3, gtsam::Cal3_S2,gtsam::CalibratedCamera, gtsam::SimpleCamera, gtsam::PinholeCameraCal3_S2, gtsam::imuBias::ConstantBias}>
virtual class PriorFactor : gtsam::NoiseModelFactor {
PriorFactor(size_t key, const T& prior, const gtsam::noiseModel::Base* noiseModel);

4
gtsam/geometry/tests/testTriangulation.cpp
View File

@ -382,8 +382,8 @@ TEST( triangulation, StereotriangulateNonlinear ) {
graph.emplace_shared<StereoFactor>(measurements[1], unit, Symbol('x',2), Symbol('l',1), stereoK);
const SharedDiagonal posePrior = noiseModel::Isotropic::Sigma(6, 1e-9);
graph.emplace_shared<PriorFactor<Pose3> >(Symbol('x',1), Pose3(m1), posePrior);
graph.emplace_shared<PriorFactor<Pose3> >(Symbol('x',2), Pose3(m2), posePrior);
graph.addPrior(Symbol('x',1), Pose3(m1), posePrior);
graph.addPrior(Symbol('x',2), Pose3(m2), posePrior);
LevenbergMarquardtOptimizer optimizer(graph, values);
Values result = optimizer.optimize();

3
gtsam/geometry/tests/testUnit3.cpp
View File

@ -27,7 +27,6 @@
#include <gtsam/nonlinear/ExpressionFactor.h>
#include <gtsam/nonlinear/GaussNewtonOptimizer.h>
#include <gtsam/nonlinear/NonlinearFactorGraph.h>
#include <gtsam/slam/PriorFactor.h>
#include <CppUnitLite/TestHarness.h>
@ -456,7 +455,7 @@ TEST(Unit3, ErrorBetweenFactor) {
// Add prior factors.
SharedNoiseModel R_prior = noiseModel::Unit::Create(2);
for (size_t i = 0; i < data.size(); i++) {
graph.emplace_shared<PriorFactor<Unit3> >(U(i), data[i], R_prior);
graph.addPrior(U(i), data[i], R_prior);
}
// Add process factors using the dot product error function.

1
gtsam/geometry/triangulation.h
View File

@ -22,7 +22,6 @@
#include <gtsam/geometry/CameraSet.h>
#include <gtsam/geometry/Pose2.h>
#include <gtsam/slam/TriangulationFactor.h>
#include <gtsam/slam/PriorFactor.h>
#include <gtsam/nonlinear/NonlinearFactorGraph.h>
#include <gtsam/inference/Symbol.h>

10
gtsam/navigation/tests/testImuFactor.cpp
View File

@ -717,7 +717,6 @@ TEST(ImuFactor, bodyPSensorNoBias) {
/* ************************************************************************* */
#include <gtsam/nonlinear/NonlinearFactorGraph.h>
#include <gtsam/slam/BetweenFactor.h>
#include <gtsam/slam/PriorFactor.h>
#include <gtsam/nonlinear/LevenbergMarquardtOptimizer.h>
#include <gtsam/nonlinear/Marginals.h>
@ -763,20 +762,17 @@ TEST(ImuFactor, bodyPSensorWithBias) {
NonlinearFactorGraph graph;
Values values;
PriorFactor<Pose3> priorPose(X(0), Pose3(), priorNoisePose);
graph.add(priorPose);
graph.addPrior(X(0), Pose3(), priorNoisePose);
values.insert(X(0), Pose3());
PriorFactor<Vector3> priorVel(V(0), zeroVel, priorNoiseVel);
graph.add(priorVel);
graph.addPrior(V(0), zeroVel, priorNoiseVel);
values.insert(V(0), zeroVel);
// The key to this test is that we specify the bias, in the sensor frame, as known a priori
// We also create factors below that encode our assumption that this bias is constant over time
// In theory, after optimization, we should recover that same bias estimate
Bias priorBias(Vector3(0, 0, 0), Vector3(0, 0.01, 0)); // Biases (acc, rot)
PriorFactor<Bias> priorBiasFactor(B(0), priorBias, priorNoiseBias);
graph.add(priorBiasFactor);
graph.addPrior(B(0), priorBias, priorNoiseBias);
values.insert(B(0), priorBias);
// Now add IMU factors and bias noise models

28
gtsam/nonlinear/NonlinearFactorGraph.h
View File

@ -24,6 +24,7 @@
#include <gtsam/geometry/Point2.h>
#include <gtsam/nonlinear/NonlinearFactor.h>
#include <gtsam/inference/FactorGraph.h>
#include <gtsam/nonlinear/PriorFactor.h>
#include <boost/shared_ptr.hpp>
#include <functional>
@ -204,6 +205,33 @@ namespace gtsam {
push_back(boost::make_shared<ExpressionFactor<T> >(R, z, h));
}
/**
* Convenience method which adds a PriorFactor to the factor graph.
* @param key Variable key
* @param prior The variable's prior value
* @param model Noise model for prior factor
*/
template<typename T>
void addPrior(Key key, const T& prior,
const SharedNoiseModel& model = nullptr) {
emplace_shared<PriorFactor<T>>(key, prior, model);
}
/**
* Convenience method which adds a PriorFactor to the factor graph.
* @param key Variable key
* @param prior The variable's prior value
* @param covariance Covariance matrix.
*
* Note that the smart noise model associated with the prior factor
* automatically picks the right noise model (e.g. a diagonal noise model
* if the provided covariance matrix is diagonal).
*/
template<typename T>
void addPrior(Key key, const T& prior, const Matrix& covariance) {
emplace_shared<PriorFactor<T>>(key, prior, covariance);
}
private:
/**

120
gtsam/nonlinear/PriorFactor.h Normal file
View File

@ -0,0 +1,120 @@
/* ----------------------------------------------------------------------------
* GTSAM Copyright 2010, Georgia Tech Research Corporation,
* Atlanta, Georgia 30332-0415
* All Rights Reserved
* Authors: Frank Dellaert, et al. (see THANKS for the full author list)
* See LICENSE for the license information
* -------------------------------------------------------------------------- */
/**
* @file PriorFactor.h
* @author Frank Dellaert
**/
#pragma once
#include <gtsam/nonlinear/NonlinearFactor.h>
#include <gtsam/base/Testable.h>
#include <string>
namespace gtsam {
/**
* A class for a soft prior on any Value type
* @addtogroup SLAM
*/
template<class VALUE>
class PriorFactor: public NoiseModelFactor1<VALUE> {
public:
typedef VALUE T;
private:
typedef NoiseModelFactor1<VALUE> Base;
VALUE prior_; /** The measurement */
/** concept check by type */
GTSAM_CONCEPT_TESTABLE_TYPE(T)
public:
/// shorthand for a smart pointer to a factor
typedef typename boost::shared_ptr<PriorFactor<VALUE> > shared_ptr;
/// Typedef to this class
typedef PriorFactor<VALUE> This;
/** default constructor - only use for serialization */
PriorFactor() {}
virtual ~PriorFactor() {}
/** Constructor */
PriorFactor(Key key, const VALUE& prior, const SharedNoiseModel& model = nullptr) :
Base(model, key), prior_(prior) {
}
/** Convenience constructor that takes a full covariance argument */
PriorFactor(Key key, const VALUE& prior, const Matrix& covariance) :
Base(noiseModel::Gaussian::Covariance(covariance), key), prior_(prior) {
}
/// @return a deep copy of this factor
virtual gtsam::NonlinearFactor::shared_ptr clone() const {
return boost::static_pointer_cast<gtsam::NonlinearFactor>(
gtsam::NonlinearFactor::shared_ptr(new This(*this))); }
/** implement functions needed for Testable */
/** print */
virtual void print(const std::string& s, const KeyFormatter& keyFormatter =
DefaultKeyFormatter) const {
std::cout << s << "PriorFactor on " << keyFormatter(this->key()) << "\n";
traits<T>::Print(prior_, " prior mean: ");
if (this->noiseModel_)
this->noiseModel_->print(" noise model: ");
else
std::cout << "no noise model" << std::endl;
}
/** equals */
virtual bool equals(const NonlinearFactor& expected, double tol=1e-9) const {
const This* e = dynamic_cast<const This*> (&expected);
return e != nullptr && Base::equals(*e, tol) && traits<T>::Equals(prior_, e->prior_, tol);
}
/** implement functions needed to derive from Factor */
/** vector of errors */
Vector evaluateError(const T& x, boost::optional<Matrix&> H = boost::none) const {
if (H) (*H) = Matrix::Identity(traits<T>::GetDimension(x),traits<T>::GetDimension(x));
// manifold equivalent of z-x -> Local(x,z)
// TODO(ASL) Add Jacobians.
return -traits<T>::Local(x, prior_);
}
const VALUE & prior() const { return prior_; }
private:
/** Serialization function */
friend class boost::serialization::access;
template<class ARCHIVE>
void serialize(ARCHIVE & ar, const unsigned int /*version*/) {
ar & boost::serialization::make_nvp("NoiseModelFactor1",
boost::serialization::base_object<Base>(*this));
ar & BOOST_SERIALIZATION_NVP(prior_);
}
// Alignment, see https://eigen.tuxfamily.org/dox/group__TopicStructHavingEigenMembers.html
enum { NeedsToAlign = (sizeof(T) % 16) == 0 };
public:
EIGEN_MAKE_ALIGNED_OPERATOR_NEW_IF(NeedsToAlign)
};
} /// namespace gtsam

2
gtsam/slam/InitializePose3.cpp
View File

@ -17,7 +17,7 @@
*/
#include <gtsam/slam/InitializePose3.h>
#include <gtsam/slam/PriorFactor.h>
#include <gtsam/nonlinear/PriorFactor.h>
#include <gtsam/slam/BetweenFactor.h>
#include <gtsam/nonlinear/GaussNewtonOptimizer.h>
#include <gtsam/inference/Symbol.h>

3
gtsam/slam/KarcherMeanFactor-inl.h
View File

@ -20,7 +20,6 @@
#include <gtsam/nonlinear/GaussNewtonOptimizer.h>
#include <gtsam/nonlinear/NonlinearFactorGraph.h>
#include <gtsam/slam/KarcherMeanFactor.h>
#include <gtsam/slam/PriorFactor.h>
using namespace std;
@ -33,7 +32,7 @@ T FindKarcherMeanImpl(const vector<T, ALLOC>& rotations) {
NonlinearFactorGraph graph;
static const Key kKey(0);
for (const auto& R : rotations) {
graph.emplace_shared<PriorFactor<T> >(kKey, R);
graph.addPrior<T>(kKey, R);
}
Values initial;
initial.insert<T>(kKey, T());

106
gtsam/slam/PriorFactor.h
View File

@ -15,106 +15,6 @@
**/
#pragma once
#include <gtsam/nonlinear/NonlinearFactor.h>
#include <gtsam/base/Testable.h>
#include <string>
namespace gtsam {
/**
* A class for a soft prior on any Value type
* @addtogroup SLAM
*/
template<class VALUE>
class PriorFactor: public NoiseModelFactor1<VALUE> {
public:
typedef VALUE T;
private:
typedef NoiseModelFactor1<VALUE> Base;
VALUE prior_; /** The measurement */
/** concept check by type */
GTSAM_CONCEPT_TESTABLE_TYPE(T)
public:
/// shorthand for a smart pointer to a factor
typedef typename boost::shared_ptr<PriorFactor<VALUE> > shared_ptr;
/// Typedef to this class
typedef PriorFactor<VALUE> This;
/** default constructor - only use for serialization */
PriorFactor() {}
virtual ~PriorFactor() {}
/** Constructor */
PriorFactor(Key key, const VALUE& prior, const SharedNoiseModel& model = nullptr) :
Base(model, key), prior_(prior) {
}
/** Convenience constructor that takes a full covariance argument */
PriorFactor(Key key, const VALUE& prior, const Matrix& covariance) :
Base(noiseModel::Gaussian::Covariance(covariance), key), prior_(prior) {
}
/// @return a deep copy of this factor
virtual gtsam::NonlinearFactor::shared_ptr clone() const {
return boost::static_pointer_cast<gtsam::NonlinearFactor>(
gtsam::NonlinearFactor::shared_ptr(new This(*this))); }
/** implement functions needed for Testable */
/** print */
virtual void print(const std::string& s, const KeyFormatter& keyFormatter =
DefaultKeyFormatter) const {
std::cout << s << "PriorFactor on " << keyFormatter(this->key()) << "\n";
traits<T>::Print(prior_, " prior mean: ");
if (this->noiseModel_)
this->noiseModel_->print(" noise model: ");
else
std::cout << "no noise model" << std::endl;
}
/** equals */
virtual bool equals(const NonlinearFactor& expected, double tol=1e-9) const {
const This* e = dynamic_cast<const This*> (&expected);
return e != nullptr && Base::equals(*e, tol) && traits<T>::Equals(prior_, e->prior_, tol);
}
/** implement functions needed to derive from Factor */
/** vector of errors */
Vector evaluateError(const T& x, boost::optional<Matrix&> H = boost::none) const {
if (H) (*H) = Matrix::Identity(traits<T>::GetDimension(x),traits<T>::GetDimension(x));
// manifold equivalent of z-x -> Local(x,z)
// TODO(ASL) Add Jacobians.
return -traits<T>::Local(x, prior_);
}
const VALUE & prior() const { return prior_; }
private:
/** Serialization function */
friend class boost::serialization::access;
template<class ARCHIVE>
void serialize(ARCHIVE & ar, const unsigned int /*version*/) {
ar & boost::serialization::make_nvp("NoiseModelFactor1",
boost::serialization::base_object<Base>(*this));
ar & BOOST_SERIALIZATION_NVP(prior_);
}
// Alignment, see https://eigen.tuxfamily.org/dox/group__TopicStructHavingEigenMembers.html
enum { NeedsToAlign = (sizeof(T) % 16) == 0 };
public:
EIGEN_MAKE_ALIGNED_OPERATOR_NEW_IF(NeedsToAlign)
};
} /// namespace gtsam
// Note: PriorFactor has been moved to gtsam/nonlinear/PriorFactor.h. This file
// has been left here for backwards compatibility.
#include <gtsam/nonlinear/PriorFactor.h>

2
gtsam/slam/lago.cpp
View File

@ -17,7 +17,7 @@
*/
#include <gtsam/slam/lago.h>
#include <gtsam/slam/PriorFactor.h>
#include <gtsam/nonlinear/PriorFactor.h>
#include <gtsam/slam/BetweenFactor.h>
#include <gtsam/inference/Symbol.h>
#include <gtsam/geometry/Pose2.h>

3
gtsam/slam/tests/testAntiFactor.cpp
View File

@ -18,7 +18,6 @@
#include <CppUnitLite/TestHarness.h>
#include <gtsam/slam/AntiFactor.h>
#include <gtsam/slam/PriorFactor.h>
#include <gtsam/slam/BetweenFactor.h>
#include <gtsam/nonlinear/NonlinearOptimizer.h>
#include <gtsam/nonlinear/NonlinearFactorGraph.h>
@ -90,7 +89,7 @@ TEST( AntiFactor, EquivalentBayesNet)
SharedNoiseModel sigma(noiseModel::Unit::Create(6));
NonlinearFactorGraph graph;
graph.emplace_shared<PriorFactor<Pose3> >(1, pose1, sigma);
graph.addPrior(1, pose1, sigma);
graph.emplace_shared<BetweenFactor<Pose3> >(1, 2, pose1.between(pose2), sigma);
// Create a configuration corresponding to the ground truth

10
gtsam/slam/tests/testGeneralSFMFactor.cpp
View File

@ -18,7 +18,6 @@
#include <gtsam/slam/GeneralSFMFactor.h>
#include <gtsam/sam/RangeFactor.h>
#include <gtsam/slam/PriorFactor.h>
#include <gtsam/geometry/Cal3_S2.h>
#include <gtsam/geometry/Rot2.h>
#include <gtsam/geometry/PinholeCamera.h>
@ -388,8 +387,7 @@ TEST(GeneralSFMFactor, GeneralCameraPoseRange) {
graph.emplace_shared<
RangeFactor<GeneralCamera, Pose3> >(X(0), X(1), 2.,
noiseModel::Isotropic::Sigma(1, 1.));
graph.emplace_shared<
PriorFactor<Pose3> >(X(1), Pose3(Rot3(), Point3(1., 0., 0.)),
graph.addPrior(X(1), Pose3(Rot3(), Point3(1., 0., 0.)),
noiseModel::Isotropic::Sigma(6, 1.));
Values init;
@ -413,14 +411,12 @@ TEST(GeneralSFMFactor, GeneralCameraPoseRange) {
TEST(GeneralSFMFactor, CalibratedCameraPoseRange) {
// Tests range factor between a CalibratedCamera and a Pose3
NonlinearFactorGraph graph;
graph.emplace_shared<
PriorFactor<CalibratedCamera> >(X(0), CalibratedCamera(),
graph.addPrior(X(0), CalibratedCamera(),
noiseModel::Isotropic::Sigma(6, 1.));
graph.emplace_shared<
RangeFactor<CalibratedCamera, Pose3> >(X(0), X(1), 2.,
noiseModel::Isotropic::Sigma(1, 1.));
graph.emplace_shared<
PriorFactor<Pose3> >(X(1), Pose3(Rot3(), Point3(1., 0., 0.)),
graph.addPrior(X(1), Pose3(Rot3(), Point3(1., 0., 0.)),
noiseModel::Isotropic::Sigma(6, 1.));
Values init;

7
gtsam/slam/tests/testInitializePose3.cpp
View File

@ -21,7 +21,6 @@
#include <gtsam/slam/InitializePose3.h>
#include <gtsam/slam/dataset.h>
#include <gtsam/slam/BetweenFactor.h>
#include <gtsam/slam/PriorFactor.h>
#include <gtsam/inference/Symbol.h>
#include <gtsam/geometry/Pose3.h>
#include <CppUnitLite/TestHarness.h>
@ -67,7 +66,7 @@ NonlinearFactorGraph graph() {
g.add(BetweenFactor<Pose3>(x2, x3, pose2.between(pose3), model));
g.add(BetweenFactor<Pose3>(x2, x0, pose2.between(pose0), model));
g.add(BetweenFactor<Pose3>(x0, x3, pose0.between(pose3), model));
g.add(PriorFactor<Pose3>(x0, pose0, model));
g.addPrior(x0, pose0, model);
return g;
}
@ -78,7 +77,7 @@ NonlinearFactorGraph graph2() {
g.add(BetweenFactor<Pose3>(x2, x3, pose2.between(pose3), noiseModel::Isotropic::Precision(6, 1.0)));
g.add(BetweenFactor<Pose3>(x2, x0, Pose3(Rot3::Ypr(0.1,0,0.1), Point3()), noiseModel::Isotropic::Precision(6, 0.0))); // random pose, but zero information
g.add(BetweenFactor<Pose3>(x0, x3, Pose3(Rot3::Ypr(0.5,-0.2,0.2), Point3(10,20,30)), noiseModel::Isotropic::Precision(6, 0.0))); // random pose, but zero informatoin
g.add(PriorFactor<Pose3>(x0, pose0, model));
g.addPrior(x0, pose0, model);
return g;
}
}
@ -267,7 +266,7 @@ TEST( InitializePose3, initializePoses )
bool is3D = true;
boost::tie(inputGraph, expectedValues) = readG2o(g2oFile, is3D);
noiseModel::Unit::shared_ptr priorModel = noiseModel::Unit::Create(6);
inputGraph->add(PriorFactor<Pose3>(0, Pose3(), priorModel));
inputGraph->addPrior(0, Pose3(), priorModel);
Values initial = InitializePose3::initialize(*inputGraph);
EXPECT(assert_equal(*expectedValues,initial,0.1)); // TODO(frank): very loose !!

15
gtsam/slam/tests/testLago.cpp
View File

@ -22,7 +22,6 @@
#include <gtsam/slam/lago.h>
#include <gtsam/slam/dataset.h>
#include <gtsam/slam/BetweenFactor.h>
#include <gtsam/slam/PriorFactor.h>
#include <gtsam/geometry/Pose2.h>
#include <gtsam/inference/Symbol.h>
@ -61,7 +60,7 @@ NonlinearFactorGraph graph() {
g.add(BetweenFactor<Pose2>(x2, x3, pose2.between(pose3), model));
g.add(BetweenFactor<Pose2>(x2, x0, pose2.between(pose0), model));
g.add(BetweenFactor<Pose2>(x0, x3, pose0.between(pose3), model));
g.add(PriorFactor<Pose2>(x0, pose0, model));
g.addPrior(x0, pose0, model);
return g;
}
}
@ -167,7 +166,7 @@ TEST( Lago, smallGraphVectorValuesSP ) {
TEST( Lago, multiplePosePriors ) {
bool useOdometricPath = false;
NonlinearFactorGraph g = simpleLago::graph();
g.add(PriorFactor<Pose2>(x1, simpleLago::pose1, model));
g.addPrior(x1, simpleLago::pose1, model);
VectorValues initial = lago::initializeOrientations(g, useOdometricPath);
// comparison is up to M_PI, that's why we add some multiples of 2*M_PI
@ -180,7 +179,7 @@ TEST( Lago, multiplePosePriors ) {
/* *************************************************************************** */
TEST( Lago, multiplePosePriorsSP ) {
NonlinearFactorGraph g = simpleLago::graph();
g.add(PriorFactor<Pose2>(x1, simpleLago::pose1, model));
g.addPrior(x1, simpleLago::pose1, model);
VectorValues initial = lago::initializeOrientations(g);
// comparison is up to M_PI, that's why we add some multiples of 2*M_PI
@ -194,7 +193,7 @@ TEST( Lago, multiplePosePriorsSP ) {
TEST( Lago, multiplePoseAndRotPriors ) {
bool useOdometricPath = false;
NonlinearFactorGraph g = simpleLago::graph();
g.add(PriorFactor<Rot2>(x1, simpleLago::pose1.theta(), model));
g.addPrior(x1, simpleLago::pose1.theta(), model);
VectorValues initial = lago::initializeOrientations(g, useOdometricPath);
// comparison is up to M_PI, that's why we add some multiples of 2*M_PI
@ -207,7 +206,7 @@ TEST( Lago, multiplePoseAndRotPriors ) {
/* *************************************************************************** */
TEST( Lago, multiplePoseAndRotPriorsSP ) {
NonlinearFactorGraph g = simpleLago::graph();
g.add(PriorFactor<Rot2>(x1, simpleLago::pose1.theta(), model));
g.addPrior(x1, simpleLago::pose1.theta(), model);
VectorValues initial = lago::initializeOrientations(g);
// comparison is up to M_PI, that's why we add some multiples of 2*M_PI
@ -267,7 +266,7 @@ TEST( Lago, largeGraphNoisy_orientations ) {
// Add prior on the pose having index (key) = 0
NonlinearFactorGraph graphWithPrior = *g;
noiseModel::Diagonal::shared_ptr priorModel = noiseModel::Diagonal::Variances(Vector3(1e-2, 1e-2, 1e-4));
graphWithPrior.add(PriorFactor<Pose2>(0, Pose2(), priorModel));
graphWithPrior.addPrior(0, Pose2(), priorModel);
VectorValues actualVV = lago::initializeOrientations(graphWithPrior);
Values actual;
@ -302,7 +301,7 @@ TEST( Lago, largeGraphNoisy ) {
// Add prior on the pose having index (key) = 0
NonlinearFactorGraph graphWithPrior = *g;
noiseModel::Diagonal::shared_ptr priorModel = noiseModel::Diagonal::Variances(Vector3(1e-2, 1e-2, 1e-4));
graphWithPrior.add(PriorFactor<Pose2>(0, Pose2(), priorModel));
graphWithPrior.addPrior(0, Pose2(), priorModel);
Values actual = lago::initialize(graphWithPrior);

9
gtsam/slam/tests/testOrientedPlane3Factor.cpp
View File

@ -17,7 +17,6 @@
*/
#include <gtsam/slam/OrientedPlane3Factor.h>
#include <gtsam/slam/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);

2
gtsam/slam/tests/testPriorFactor.cpp
View File

@ -6,7 +6,7 @@
*/
#include <gtsam/base/Vector.h>
#include <gtsam/slam/PriorFactor.h>
#include <gtsam/nonlinear/PriorFactor.h>
#include <CppUnitLite/TestHarness.h>
using namespace std;

20
gtsam/slam/tests/testSmartProjectionFactor.cpp
View File

@ -195,8 +195,8 @@ TEST(SmartProjectionFactor, perturbPoseAndOptimize ) {
graph.push_back(smartFactor2);
graph.push_back(smartFactor3);
const SharedDiagonal noisePrior = noiseModel::Isotropic::Sigma(6 + 5, 1e-5);
graph.emplace_shared<PriorFactor<Camera> >(c1, cam1, noisePrior);
graph.emplace_shared<PriorFactor<Camera> >(c2, cam2, noisePrior);
graph.addPrior(c1, cam1, noisePrior);
graph.addPrior(c2, cam2, noisePrior);
// Create initial estimate
Values initial;
@ -304,8 +304,8 @@ TEST(SmartProjectionFactor, perturbPoseAndOptimizeFromSfM_tracks ) {
graph.push_back(smartFactor1);
graph.push_back(smartFactor2);
graph.push_back(smartFactor3);
graph.emplace_shared<PriorFactor<Camera> >(c1, cam1, noisePrior);
graph.emplace_shared<PriorFactor<Camera> >(c2, cam2, noisePrior);
graph.addPrior(c1, cam1, noisePrior);
graph.addPrior(c2, cam2, noisePrior);
Values values;
values.insert(c1, cam1);
@ -378,8 +378,8 @@ TEST(SmartProjectionFactor, perturbCamerasAndOptimize ) {
graph.push_back(smartFactor3);
graph.push_back(smartFactor4);
graph.push_back(smartFactor5);
graph.emplace_shared<PriorFactor<Camera> >(c1, cam1, noisePrior);
graph.emplace_shared<PriorFactor<Camera> >(c2, cam2, noisePrior);
graph.addPrior(c1, cam1, noisePrior);
graph.addPrior(c2, cam2, noisePrior);
Values values;
values.insert(c1, cam1);
@ -453,8 +453,8 @@ TEST(SmartProjectionFactor, Cal3Bundler ) {
graph.push_back(smartFactor1);
graph.push_back(smartFactor2);
graph.push_back(smartFactor3);
graph.emplace_shared<PriorFactor<Camera> >(c1, cam1, noisePrior);
graph.emplace_shared<PriorFactor<Camera> >(c2, cam2, noisePrior);
graph.addPrior(c1, cam1, noisePrior);
graph.addPrior(c2, cam2, noisePrior);
Values values;
values.insert(c1, cam1);
@ -526,8 +526,8 @@ TEST(SmartProjectionFactor, Cal3Bundler2 ) {
graph.push_back(smartFactor1);
graph.push_back(smartFactor2);
graph.push_back(smartFactor3);
graph.emplace_shared<PriorFactor<Camera> >(c1, cam1, noisePrior);
graph.emplace_shared<PriorFactor<Camera> >(c2, cam2, noisePrior);
graph.addPrior(c1, cam1, noisePrior);
graph.addPrior(c2, cam2, noisePrior);
Values values;
values.insert(c1, cam1);

42
gtsam/slam/tests/testSmartProjectionPoseFactor.cpp
View File

@ -243,8 +243,8 @@ TEST(SmartProjectionPoseFactor, smartFactorWithSensorBodyTransform) {
graph.push_back(smartFactor1);
graph.push_back(smartFactor2);
graph.push_back(smartFactor3);
graph.emplace_shared<PriorFactor<Pose3> >(x1, wTb1, noisePrior);
graph.emplace_shared<PriorFactor<Pose3> >(x2, wTb2, noisePrior);
graph.addPrior(x1, wTb1, noisePrior);
graph.addPrior(x2, wTb2, noisePrior);
// Check errors at ground truth poses
Values gtValues;
@ -302,8 +302,8 @@ TEST( SmartProjectionPoseFactor, 3poses_smart_projection_factor ) {
graph.push_back(smartFactor1);
graph.push_back(smartFactor2);
graph.push_back(smartFactor3);
graph.emplace_shared<PriorFactor<Pose3> >(x1, cam1.pose(), noisePrior);
graph.emplace_shared<PriorFactor<Pose3> >(x2, cam2.pose(), noisePrior);
graph.addPrior(x1, cam1.pose(), noisePrior);
graph.addPrior(x2, cam2.pose(), noisePrior);
Values groundTruth;
groundTruth.insert(x1, cam1.pose());
@ -525,8 +525,8 @@ TEST( SmartProjectionPoseFactor, 3poses_iterative_smart_projection_factor ) {
graph.push_back(smartFactor1);
graph.push_back(smartFactor2);
graph.push_back(smartFactor3);
graph.emplace_shared<PriorFactor<Pose3> >(x1, cam1.pose(), noisePrior);
graph.emplace_shared<PriorFactor<Pose3> >(x2, cam2.pose(), noisePrior);
graph.addPrior(x1, cam1.pose(), noisePrior);
graph.addPrior(x2, cam2.pose(), noisePrior);
// Pose3 noise_pose = Pose3(Rot3::Ypr(-M_PI/10, 0., -M_PI/10), Point3(0.5,0.1,0.3)); // noise from regular projection factor test below
Pose3 noise_pose = Pose3(Rot3::Ypr(-M_PI / 100, 0., -M_PI / 100),
@ -589,8 +589,8 @@ TEST( SmartProjectionPoseFactor, jacobianSVD ) {
graph.push_back(smartFactor1);
graph.push_back(smartFactor2);
graph.push_back(smartFactor3);
graph.emplace_shared<PriorFactor<Pose3> >(x1, cam1.pose(), noisePrior);
graph.emplace_shared<PriorFactor<Pose3> >(x2, cam2.pose(), noisePrior);
graph.addPrior(x1, cam1.pose(), noisePrior);
graph.addPrior(x2, cam2.pose(), noisePrior);
// Pose3 noise_pose = Pose3(Rot3::Ypr(-M_PI/10, 0., -M_PI/10), Point3(0.5,0.1,0.3)); // noise from regular projection factor test below
Pose3 noise_pose = Pose3(Rot3::Ypr(-M_PI / 100, 0., -M_PI / 100),
@ -647,8 +647,8 @@ TEST( SmartProjectionPoseFactor, landmarkDistance ) {
graph.push_back(smartFactor1);
graph.push_back(smartFactor2);
graph.push_back(smartFactor3);
graph.emplace_shared<PriorFactor<Pose3> >(x1, cam1.pose(), noisePrior);
graph.emplace_shared<PriorFactor<Pose3> >(x2, cam2.pose(), noisePrior);
graph.addPrior(x1, cam1.pose(), noisePrior);
graph.addPrior(x2, cam2.pose(), noisePrior);
// Pose3 noise_pose = Pose3(Rot3::Ypr(-M_PI/10, 0., -M_PI/10), Point3(0.5,0.1,0.3)); // noise from regular projection factor test below
Pose3 noise_pose = Pose3(Rot3::Ypr(-M_PI / 100, 0., -M_PI / 100),
@ -716,8 +716,8 @@ TEST( SmartProjectionPoseFactor, dynamicOutlierRejection ) {
graph.push_back(smartFactor2);
graph.push_back(smartFactor3);
graph.push_back(smartFactor4);
graph.emplace_shared<PriorFactor<Pose3> >(x1, cam1.pose(), noisePrior);
graph.emplace_shared<PriorFactor<Pose3> >(x2, cam2.pose(), noisePrior);
graph.addPrior(x1, cam1.pose(), noisePrior);
graph.addPrior(x2, cam2.pose(), noisePrior);
Values values;
values.insert(x1, cam1.pose());
@ -766,8 +766,8 @@ TEST( SmartProjectionPoseFactor, jacobianQ ) {
graph.push_back(smartFactor1);
graph.push_back(smartFactor2);
graph.push_back(smartFactor3);
graph.emplace_shared<PriorFactor<Pose3> >(x1, cam1.pose(), noisePrior);
graph.emplace_shared<PriorFactor<Pose3> >(x2, cam2.pose(), noisePrior);
graph.addPrior(x1, cam1.pose(), noisePrior);
graph.addPrior(x2, cam2.pose(), noisePrior);
Pose3 noise_pose = Pose3(Rot3::Ypr(-M_PI / 100, 0., -M_PI / 100),
Point3(0.1, 0.1, 0.1)); // smaller noise
@ -806,8 +806,8 @@ TEST( SmartProjectionPoseFactor, 3poses_projection_factor ) {
graph.emplace_shared<ProjectionFactor>(cam3.project(landmark3), model, x3, L(3), sharedK2);
const SharedDiagonal noisePrior = noiseModel::Isotropic::Sigma(6, 0.10);
graph.emplace_shared<PriorFactor<Pose3> >(x1, level_pose, noisePrior);
graph.emplace_shared<PriorFactor<Pose3> >(x2, pose_right, noisePrior);
graph.addPrior(x1, level_pose, noisePrior);
graph.addPrior(x2, pose_right, noisePrior);
Pose3 noise_pose = Pose3(Rot3::Ypr(-M_PI / 10, 0., -M_PI / 10),
Point3(0.5, 0.1, 0.3));
@ -948,7 +948,7 @@ TEST( SmartProjectionPoseFactor, 3poses_2land_rotation_only_smart_projection_fac
NonlinearFactorGraph graph;
graph.push_back(smartFactor1);
graph.push_back(smartFactor2);
graph.emplace_shared<PriorFactor<Pose3> >(x1, cam1.pose(), noisePrior);
graph.addPrior(x1, cam1.pose(), noisePrior);
graph.emplace_shared<PoseTranslationPrior<Pose3> >(x2, positionPrior, noisePriorTranslation);
graph.emplace_shared<PoseTranslationPrior<Pose3> >(x3, positionPrior, noisePriorTranslation);
@ -1012,7 +1012,7 @@ TEST( SmartProjectionPoseFactor, 3poses_rotation_only_smart_projection_factor )
graph.push_back(smartFactor1);
graph.push_back(smartFactor2);
graph.push_back(smartFactor3);
graph.emplace_shared<PriorFactor<Pose3> >(x1, cam1.pose(), noisePrior);
graph.addPrior(x1, cam1.pose(), noisePrior);
graph.emplace_shared<PoseTranslationPrior<Pose3> >(x2, positionPrior, noisePriorTranslation);
graph.emplace_shared<PoseTranslationPrior<Pose3> >(x3, positionPrior, noisePriorTranslation);
@ -1223,8 +1223,8 @@ TEST( SmartProjectionPoseFactor, Cal3Bundler ) {
graph.push_back(smartFactor1);
graph.push_back(smartFactor2);
graph.push_back(smartFactor3);
graph.emplace_shared<PriorFactor<Pose3> >(x1, cam1.pose(), noisePrior);
graph.emplace_shared<PriorFactor<Pose3> >(x2, cam2.pose(), noisePrior);
graph.addPrior(x1, cam1.pose(), noisePrior);
graph.addPrior(x2, cam2.pose(), noisePrior);
// Pose3 noise_pose = Pose3(Rot3::Ypr(-M_PI/10, 0., -M_PI/10), Point3(0.5,0.1,0.3)); // noise from regular projection factor test below
Pose3 noise_pose = Pose3(Rot3::Ypr(-M_PI / 100, 0., -M_PI / 100),
@ -1296,7 +1296,7 @@ TEST( SmartProjectionPoseFactor, Cal3BundlerRotationOnly ) {
graph.push_back(smartFactor1);
graph.push_back(smartFactor2);
graph.push_back(smartFactor3);
graph.emplace_shared<PriorFactor<Pose3> >(x1, cam1.pose(), noisePrior);
graph.addPrior(x1, cam1.pose(), noisePrior);
graph.emplace_shared<PoseTranslationPrior<Pose3> >(x2, positionPrior, noisePriorTranslation);
graph.emplace_shared<PoseTranslationPrior<Pose3> >(x3, positionPrior, noisePriorTranslation);

2
gtsam_unstable/dynamics/tests/testIMUSystem.cpp
View File

@ -8,7 +8,7 @@
#include <CppUnitLite/TestHarness.h>
#include <gtsam/slam/BetweenFactor.h>
#include <gtsam/slam/PriorFactor.h>
#include <gtsam/nonlinear/PriorFactor.h>
#include <gtsam/sam/RangeFactor.h>
#include <gtsam_unstable/slam/PartialPriorFactor.h>
#include <gtsam/nonlinear/NonlinearEquality.h>

5
gtsam_unstable/examples/ConcurrentCalibration.cpp
View File

@ -26,7 +26,6 @@
#include <gtsam/inference/Symbol.h>
#include <gtsam/slam/ProjectionFactor.h>
#include <gtsam/slam/GeneralSFMFactor.h>
#include <gtsam/slam/PriorFactor.h>
#include <gtsam/slam/dataset.h>
#include <string>
@ -61,7 +60,7 @@ int main(int argc, char** argv){
initial_estimate.insert(Symbol('K', 0), *noisy_K);
noiseModel::Diagonal::shared_ptr calNoise = noiseModel::Diagonal::Sigmas((Vector(5) << 500, 500, 1e-5, 100, 100).finished());
graph.emplace_shared<PriorFactor<Cal3_S2> >(Symbol('K', 0), *noisy_K, calNoise);
graph.addPrior(Symbol('K', 0), *noisy_K, calNoise);
ifstream pose_file(pose_loc.c_str());
@ -77,7 +76,7 @@ int main(int argc, char** argv){
}
noiseModel::Isotropic::shared_ptr poseNoise = noiseModel::Isotropic::Sigma(6, 0.01);
graph.emplace_shared<PriorFactor<Pose3> >(Symbol('x', pose_id), Pose3(m), poseNoise);
graph.addPrior(Symbol('x', pose_id), Pose3(m), poseNoise);
// camera and landmark keys
size_t x, l;

3
gtsam_unstable/examples/ConcurrentFilteringAndSmoothingExample.cpp
View File

@ -34,7 +34,6 @@
// have been provided with the library for solving robotics/SLAM/Bundle Adjustment problems.
// Here we will use Between factors for the relative motion described by odometry measurements.
// Also, we will initialize the robot at the origin using a Prior factor.
#include <gtsam/slam/PriorFactor.h>
#include <gtsam/slam/BetweenFactor.h>
// When the factors are created, we will add them to a Factor Graph. As the factors we are using
@ -86,7 +85,7 @@ int main(int argc, char** argv) {
Pose2 priorMean(0.0, 0.0, 0.0); // prior at origin
auto priorNoise = noiseModel::Diagonal::Sigmas(Vector3(0.3, 0.3, 0.1));
Key priorKey = 0;
newFactors.push_back(PriorFactor<Pose2>(priorKey, priorMean, priorNoise));
newFactors.addPrior(priorKey, priorMean, priorNoise);
newValues.insert(priorKey, priorMean); // Initialize the first pose at the mean of the prior
newTimestamps[priorKey] = 0.0; // Set the timestamp associated with this key to 0.0 seconds;

3
gtsam_unstable/examples/FixedLagSmootherExample.cpp
View File

@ -30,7 +30,6 @@
// have been provided with the library for solving robotics/SLAM/Bundle Adjustment problems.
// Here we will use Between factors for the relative motion described by odometry measurements.
// Also, we will initialize the robot at the origin using a Prior factor.
#include <gtsam/slam/PriorFactor.h>
#include <gtsam/slam/BetweenFactor.h>
// When the factors are created, we will add them to a Factor Graph. As the factors we are using
@ -80,7 +79,7 @@ int main(int argc, char** argv) {
Pose2 priorMean(0.0, 0.0, 0.0); // prior at origin
noiseModel::Diagonal::shared_ptr priorNoise = noiseModel::Diagonal::Sigmas(Vector3(0.3, 0.3, 0.1));
Key priorKey = 0;
newFactors.push_back(PriorFactor<Pose2>(priorKey, priorMean, priorNoise));
newFactors.addPrior(priorKey, priorMean, priorNoise);
newValues.insert(priorKey, priorMean); // Initialize the first pose at the mean of the prior
newTimestamps[priorKey] = 0.0; // Set the timestamp associated with this key to 0.0 seconds;

3
gtsam_unstable/examples/SmartRangeExample_plaza1.cpp
View File

@ -37,7 +37,6 @@
// In GTSAM, measurement functions are represented as 'factors'. Several common factors
// have been provided with the library for solving robotics SLAM problems.
#include <gtsam/slam/PriorFactor.h>
#include <gtsam/slam/BetweenFactor.h>
#include <gtsam/sam/RangeFactor.h>
#include <gtsam_unstable/slam/SmartRangeFactor.h>
@ -127,7 +126,7 @@ int main(int argc, char** argv) {
Pose2 pose0 = Pose2(-34.2086489999201, 45.3007639991120,
M_PI - 2.02108900000000);
NonlinearFactorGraph newFactors;
newFactors.push_back(PriorFactor<Pose2>(0, pose0, priorNoise));
newFactors.addPrior(0, pose0, priorNoise);
ofstream os2("rangeResultLM.txt");
ofstream os3("rangeResultSR.txt");

3
gtsam_unstable/examples/SmartRangeExample_plaza2.cpp
View File

@ -37,7 +37,6 @@
// In GTSAM, measurement functions are represented as 'factors'. Several common factors
// have been provided with the library for solving robotics SLAM problems.
#include <gtsam/slam/PriorFactor.h>
#include <gtsam/slam/BetweenFactor.h>
#include <gtsam/sam/RangeFactor.h>
@ -124,7 +123,7 @@ int main(int argc, char** argv) {
// Add prior on first pose
Pose2 pose0 = Pose2(-34.2086489999201, 45.3007639991120, -2.02108900000000);
NonlinearFactorGraph newFactors;
newFactors.push_back(PriorFactor<Pose2>(0, pose0, priorNoise));
newFactors.addPrior(0, pose0, priorNoise);
// initialize points (Gaussian)
Values initial;

7
gtsam_unstable/geometry/tests/testSimilarity3.cpp
View File

@ -17,7 +17,6 @@
*/
#include <gtsam_unstable/geometry/Similarity3.h>
#include <gtsam/slam/PriorFactor.h>
#include <gtsam/slam/BetweenFactor.h>
#include <gtsam/nonlinear/NonlinearFactorGraph.h>
#include <gtsam/nonlinear/ExpressionFactorGraph.h>
@ -263,11 +262,10 @@ TEST(Similarity3, Optimization) {
Similarity3 prior = Similarity3(Rot3::Ypr(0.1, 0.2, 0.3), Point3(1, 2, 3), 4);
noiseModel::Isotropic::shared_ptr model = noiseModel::Isotropic::Sigma(7, 1);
Symbol key('x', 1);
PriorFactor<Similarity3> factor(key, prior, model);
// Create graph
NonlinearFactorGraph graph;
graph.push_back(factor);
graph.addPrior(key, prior, model);
// Create initial estimate with identity transform
Values initial;
@ -304,7 +302,6 @@ TEST(Similarity3, Optimization2) {
(Vector(7) << 0.1, 0.1, 0.1, 0.1, 0.1, 0.1, 10).finished());
SharedDiagonal betweenNoise2 = noiseModel::Diagonal::Sigmas(
(Vector(7) << 0.1, 0.1, 0.1, 0.1, 0.1, 0.1, 1.0).finished());
PriorFactor<Similarity3> factor(X(1), prior, model); // Prior !
BetweenFactor<Similarity3> b1(X(1), X(2), m1, betweenNoise);
BetweenFactor<Similarity3> b2(X(2), X(3), m2, betweenNoise);
BetweenFactor<Similarity3> b3(X(3), X(4), m3, betweenNoise);
@ -313,7 +310,7 @@ TEST(Similarity3, Optimization2) {
// Create graph
NonlinearFactorGraph graph;
graph.push_back(factor);
graph.addPrior(X(1), prior, model); // Prior !
graph.push_back(b1);
graph.push_back(b2);
graph.push_back(b3);

2
gtsam_unstable/gtsam_unstable.h
View File

@ -274,7 +274,7 @@ class SimPolygon2D {
};
// Nonlinear factors from gtsam, for our Value types
#include <gtsam/slam/PriorFactor.h>
#include <gtsam/nonlinear/PriorFactor.h>
template<T = {gtsam::PoseRTV}>
virtual class PriorFactor : gtsam::NoiseModelFactor {
PriorFactor(size_t key, const T& prior, const gtsam::noiseModel::Base* noiseModel);

3
gtsam_unstable/nonlinear/tests/testBatchFixedLagSmoother.cpp
View File

@ -26,7 +26,6 @@
#include <gtsam/linear/GaussianFactorGraph.h>
#include <gtsam/nonlinear/NonlinearFactorGraph.h>
#include <gtsam/nonlinear/Values.h>
#include <gtsam/slam/PriorFactor.h>
#include <gtsam/slam/BetweenFactor.h>
using namespace std;
@ -84,7 +83,7 @@ TEST( BatchFixedLagSmoother, Example )
Values newValues;
Timestamps newTimestamps;
newFactors.push_back(PriorFactor<Point2>(key0, Point2(0.0, 0.0), odometerNoise));
newFactors.addPrior(key0, Point2(0.0, 0.0), odometerNoise);
newValues.insert(key0, Point2(0.01, 0.01));
newTimestamps[key0] = 0.0;

34
gtsam_unstable/nonlinear/tests/testConcurrentBatchFilter.cpp
View File

@ -17,7 +17,7 @@
*/
#include <gtsam_unstable/nonlinear/ConcurrentBatchFilter.h>
#include <gtsam/slam/PriorFactor.h>
#include <gtsam/nonlinear/PriorFactor.h>
#include <gtsam/slam/BetweenFactor.h>
#include <gtsam/nonlinear/ISAM2.h>
#include <gtsam/nonlinear/LevenbergMarquardtOptimizer.h>
@ -134,7 +134,7 @@ TEST( ConcurrentBatchFilter, getFactors )
// Add some factors to the filter
NonlinearFactorGraph newFactors1;
newFactors1.push_back(PriorFactor<Pose3>(1, poseInitial, noisePrior));
newFactors1.addPrior(1, poseInitial, noisePrior);
newFactors1.push_back(BetweenFactor<Pose3>(1, 2, poseOdometry, noiseOdometery));
Values newValues1;
newValues1.insert(1, Pose3());
@ -184,7 +184,7 @@ TEST( ConcurrentBatchFilter, getLinearizationPoint )
// Add some factors to the filter
NonlinearFactorGraph newFactors1;
newFactors1.push_back(PriorFactor<Pose3>(1, poseInitial, noisePrior));
newFactors1.addPrior(1, poseInitial, noisePrior);
newFactors1.push_back(BetweenFactor<Pose3>(1, 2, poseOdometry, noiseOdometery));
Values newValues1;
newValues1.insert(1, Pose3());
@ -246,7 +246,7 @@ TEST( ConcurrentBatchFilter, calculateEstimate )
// Add some factors to the filter
NonlinearFactorGraph newFactors2;
newFactors2.push_back(PriorFactor<Pose3>(1, poseInitial, noisePrior));
newFactors2.addPrior(1, poseInitial, noisePrior);
newFactors2.push_back(BetweenFactor<Pose3>(1, 2, poseOdometry, noiseOdometery));
Values newValues2;
newValues2.insert(1, Pose3().compose(poseError));
@ -330,7 +330,7 @@ TEST( ConcurrentBatchFilter, update_multiple )
// Add some factors to the filter
NonlinearFactorGraph newFactors2;
newFactors2.push_back(PriorFactor<Pose3>(1, poseInitial, noisePrior));
newFactors2.addPrior(1, poseInitial, noisePrior);
newFactors2.push_back(BetweenFactor<Pose3>(1, 2, poseOdometry, noiseOdometery));
Values newValues2;
newValues2.insert(1, Pose3().compose(poseError));
@ -380,7 +380,7 @@ TEST( ConcurrentBatchFilter, update_and_marginalize )
// Add some factors to the filter
NonlinearFactorGraph newFactors;
newFactors.push_back(PriorFactor<Pose3>(1, poseInitial, noisePrior));
newFactors.addPrior(1, poseInitial, noisePrior);
newFactors.push_back(BetweenFactor<Pose3>(1, 2, poseOdometry, noiseOdometery));
newFactors.push_back(BetweenFactor<Pose3>(2, 3, poseOdometry, noiseOdometery));
newFactors.push_back(BetweenFactor<Pose3>(3, 4, poseOdometry, noiseOdometery));
@ -403,7 +403,7 @@ TEST( ConcurrentBatchFilter, update_and_marginalize )
NonlinearFactorGraph partialGraph;
partialGraph.emplace_shared<PriorFactor<Pose3> >(1, poseInitial, noisePrior);
partialGraph.addPrior(1, poseInitial, noisePrior);
partialGraph.emplace_shared<BetweenFactor<Pose3> >(1, 2, poseOdometry, noiseOdometery);
partialGraph.emplace_shared<BetweenFactor<Pose3> >(2, 3, poseOdometry, noiseOdometery);
@ -504,7 +504,7 @@ TEST( ConcurrentBatchFilter, synchronize_1 )
// Insert factors into the filter, but do not marginalize out any variables.
// The synchronization should still be empty
NonlinearFactorGraph newFactors;
newFactors.push_back(PriorFactor<Pose3>(1, poseInitial, noisePrior));
newFactors.addPrior(1, poseInitial, noisePrior);
newFactors.push_back(BetweenFactor<Pose3>(1, 2, poseOdometry, noiseOdometery));
Values newValues;
newValues.insert(1, Pose3().compose(poseError));
@ -1090,7 +1090,7 @@ TEST( ConcurrentBatchFilter, removeFactors_topology_1 )
// Add some factors to the filter
NonlinearFactorGraph newFactors;
newFactors.push_back(PriorFactor<Pose3>(1, poseInitial, noisePrior));
newFactors.addPrior(1, poseInitial, noisePrior);
newFactors.push_back(BetweenFactor<Pose3>(1, 2, poseOdometry, noiseOdometery));
newFactors.push_back(BetweenFactor<Pose3>(2, 3, poseOdometry, noiseOdometery));
newFactors.push_back(BetweenFactor<Pose3>(3, 4, poseOdometry, noiseOdometery));
@ -1121,7 +1121,7 @@ TEST( ConcurrentBatchFilter, removeFactors_topology_1 )
NonlinearFactorGraph actualGraph = filter.getFactors();
NonlinearFactorGraph expectedGraph;
expectedGraph.emplace_shared<PriorFactor<Pose3> >(1, poseInitial, noisePrior);
expectedGraph.addPrior(1, poseInitial, noisePrior);
// we removed this one: expectedGraph.emplace_shared<BetweenFactor<Pose3> >(1, 2, poseOdometry, noiseOdometery);
// we should add an empty one, so that the ordering and labeling of the factors is preserved
expectedGraph.push_back(NonlinearFactor::shared_ptr());
@ -1145,7 +1145,7 @@ TEST( ConcurrentBatchFilter, removeFactors_topology_2 )
// Add some factors to the filter
NonlinearFactorGraph newFactors;
newFactors.push_back(PriorFactor<Pose3>(1, poseInitial, noisePrior));
newFactors.addPrior(1, poseInitial, noisePrior);
newFactors.push_back(BetweenFactor<Pose3>(1, 2, poseOdometry, noiseOdometery));
newFactors.push_back(BetweenFactor<Pose3>(2, 3, poseOdometry, noiseOdometery));
newFactors.push_back(BetweenFactor<Pose3>(3, 4, poseOdometry, noiseOdometery));
@ -1176,7 +1176,7 @@ TEST( ConcurrentBatchFilter, removeFactors_topology_2 )
NonlinearFactorGraph actualGraph = filter.getFactors();
NonlinearFactorGraph expectedGraph;
expectedGraph.emplace_shared<PriorFactor<Pose3> >(1, poseInitial, noisePrior);
expectedGraph.addPrior(1, poseInitial, noisePrior);
expectedGraph.emplace_shared<BetweenFactor<Pose3> >(1, 2, poseOdometry, noiseOdometery);
expectedGraph.emplace_shared<BetweenFactor<Pose3> >(2, 3, poseOdometry, noiseOdometery);
expectedGraph.emplace_shared<BetweenFactor<Pose3> >(3, 4, poseOdometry, noiseOdometery);
@ -1200,8 +1200,8 @@ TEST( ConcurrentBatchFilter, removeFactors_topology_3 )
// Add some factors to the filter
NonlinearFactorGraph newFactors;
newFactors.push_back(PriorFactor<Pose3>(1, poseInitial, noisePrior));
newFactors.push_back(PriorFactor<Pose3>(1, poseInitial, noisePrior));
newFactors.addPrior(1, poseInitial, noisePrior);
newFactors.addPrior(1, poseInitial, noisePrior);
newFactors.push_back(BetweenFactor<Pose3>(1, 2, poseOdometry, noiseOdometery));
newFactors.push_back(BetweenFactor<Pose3>(2, 3, poseOdometry, noiseOdometery));
newFactors.push_back(BetweenFactor<Pose3>(3, 4, poseOdometry, noiseOdometery));
@ -1233,7 +1233,7 @@ TEST( ConcurrentBatchFilter, removeFactors_topology_3 )
NonlinearFactorGraph expectedGraph;
// we should add an empty one, so that the ordering and labeling of the factors is preserved
expectedGraph.push_back(NonlinearFactor::shared_ptr());
expectedGraph.emplace_shared<PriorFactor<Pose3> >(1, poseInitial, noisePrior);
expectedGraph.addPrior(1, poseInitial, noisePrior);
expectedGraph.emplace_shared<BetweenFactor<Pose3> >(1, 2, poseOdometry, noiseOdometery);
expectedGraph.emplace_shared<BetweenFactor<Pose3> >(2, 3, poseOdometry, noiseOdometery);
expectedGraph.emplace_shared<BetweenFactor<Pose3> >(3, 4, poseOdometry, noiseOdometery);
@ -1253,7 +1253,7 @@ TEST( ConcurrentBatchFilter, removeFactors_values )
// Add some factors to the filter
NonlinearFactorGraph newFactors;
newFactors.emplace_shared<PriorFactor<Pose3> >(1, poseInitial, noisePrior);
newFactors.addPrior(1, poseInitial, noisePrior);
newFactors.emplace_shared<BetweenFactor<Pose3> >(1, 2, poseOdometry, noiseOdometery);
newFactors.emplace_shared<BetweenFactor<Pose3> >(2, 3, poseOdometry, noiseOdometery);
newFactors.emplace_shared<BetweenFactor<Pose3> >(3, 4, poseOdometry, noiseOdometery);
@ -1286,7 +1286,7 @@ TEST( ConcurrentBatchFilter, removeFactors_values )
// note: factors are removed before the optimization
NonlinearFactorGraph expectedGraph;
expectedGraph.emplace_shared<PriorFactor<Pose3> >(1, poseInitial, noisePrior);
expectedGraph.addPrior(1, poseInitial, noisePrior);
expectedGraph.emplace_shared<BetweenFactor<Pose3> >(1, 2, poseOdometry, noiseOdometery);
expectedGraph.emplace_shared<BetweenFactor<Pose3> >(2, 3, poseOdometry, noiseOdometery);
expectedGraph.emplace_shared<BetweenFactor<Pose3> >(3, 4, poseOdometry, noiseOdometery);

30
gtsam_unstable/nonlinear/tests/testConcurrentBatchSmoother.cpp
View File

@ -17,7 +17,7 @@
*/
#include <gtsam_unstable/nonlinear/ConcurrentBatchSmoother.h>
#include <gtsam/slam/PriorFactor.h>
#include <gtsam/nonlinear/PriorFactor.h>
#include <gtsam/slam/BetweenFactor.h>
#include <gtsam/nonlinear/ISAM2.h>
#include <gtsam/nonlinear/LevenbergMarquardtOptimizer.h>
@ -104,7 +104,7 @@ TEST( ConcurrentBatchSmoother, getFactors )
// Add some factors to the smoother
NonlinearFactorGraph newFactors1;
newFactors1.push_back(PriorFactor<Pose3>(1, poseInitial, noisePrior));
newFactors1.addPrior(1, poseInitial, noisePrior);
newFactors1.push_back(BetweenFactor<Pose3>(1, 2, poseOdometry, noiseOdometery));
Values newValues1;
newValues1.insert(1, Pose3());
@ -154,7 +154,7 @@ TEST( ConcurrentBatchSmoother, getLinearizationPoint )
// Add some factors to the smoother
NonlinearFactorGraph newFactors1;
newFactors1.push_back(PriorFactor<Pose3>(1, poseInitial, noisePrior));
newFactors1.addPrior(1, poseInitial, noisePrior);
newFactors1.push_back(BetweenFactor<Pose3>(1, 2, poseOdometry, noiseOdometery));
Values newValues1;
newValues1.insert(1, Pose3());
@ -216,7 +216,7 @@ TEST( ConcurrentBatchSmoother, calculateEstimate )
// Add some factors to the smoother
NonlinearFactorGraph newFactors2;
newFactors2.push_back(PriorFactor<Pose3>(1, poseInitial, noisePrior));
newFactors2.addPrior(1, poseInitial, noisePrior);
newFactors2.push_back(BetweenFactor<Pose3>(1, 2, poseOdometry, noiseOdometery));
Values newValues2;
newValues2.insert(1, Pose3().compose(poseError));
@ -302,7 +302,7 @@ TEST( ConcurrentBatchSmoother, update_multiple )
// Add some factors to the smoother
NonlinearFactorGraph newFactors2;
newFactors2.push_back(PriorFactor<Pose3>(1, poseInitial, noisePrior));
newFactors2.addPrior(1, poseInitial, noisePrior);
newFactors2.push_back(BetweenFactor<Pose3>(1, 2, poseOdometry, noiseOdometery));
Values newValues2;
newValues2.insert(1, Pose3().compose(poseError));
@ -527,7 +527,7 @@ TEST( ConcurrentBatchSmoother, synchronize_3 )
filterSumarization.push_back(LinearContainerFactor(BetweenFactor<Pose3>(1, 2, poseOdometry, noiseOdometery).linearize(filterSeparatorValues), filterSeparatorValues));
smootherFactors.push_back(BetweenFactor<Pose3>(2, 3, poseOdometry, noiseOdometery));
smootherFactors.push_back(BetweenFactor<Pose3>(3, 4, poseOdometry, noiseOdometery));
smootherFactors.push_back(PriorFactor<Pose3>(4, poseInitial, noisePrior));
smootherFactors.addPrior(4, poseInitial, noisePrior);
smootherValues.insert(3, filterSeparatorValues.at<Pose3>(2).compose(poseOdometry).compose(poseError));
smootherValues.insert(4, smootherValues.at<Pose3>(3).compose(poseOdometry).compose(poseError));
@ -588,7 +588,7 @@ TEST( ConcurrentBatchSmoother, removeFactors_topology_1 )
// Add some factors to the smoother
NonlinearFactorGraph newFactors;
newFactors.push_back(PriorFactor<Pose3>(1, poseInitial, noisePrior));
newFactors.addPrior(1, poseInitial, noisePrior);
newFactors.push_back(BetweenFactor<Pose3>(1, 2, poseOdometry, noiseOdometery));
newFactors.push_back(BetweenFactor<Pose3>(2, 3, poseOdometry, noiseOdometery));
newFactors.push_back(BetweenFactor<Pose3>(3, 4, poseOdometry, noiseOdometery));
@ -617,7 +617,7 @@ TEST( ConcurrentBatchSmoother, removeFactors_topology_1 )
NonlinearFactorGraph actualGraph = smoother.getFactors();
NonlinearFactorGraph expectedGraph;
expectedGraph.emplace_shared<PriorFactor<Pose3> >(1, poseInitial, noisePrior);
expectedGraph.addPrior(1, poseInitial, noisePrior);
// we removed this one: expectedGraph.emplace_shared<BetweenFactor<Pose3> >(1, 2, poseOdometry, noiseOdometery);
// we should add an empty one, so that the ordering and labeling of the factors is preserved
expectedGraph.push_back(NonlinearFactor::shared_ptr());
@ -642,7 +642,7 @@ TEST( ConcurrentBatchSmoother, removeFactors_topology_2 )
// Add some factors to the smoother
NonlinearFactorGraph newFactors;
newFactors.push_back(PriorFactor<Pose3>(1, poseInitial, noisePrior));
newFactors.addPrior(1, poseInitial, noisePrior);
newFactors.push_back(BetweenFactor<Pose3>(1, 2, poseOdometry, noiseOdometery));
newFactors.push_back(BetweenFactor<Pose3>(2, 3, poseOdometry, noiseOdometery));
newFactors.push_back(BetweenFactor<Pose3>(3, 4, poseOdometry, noiseOdometery));
@ -670,7 +670,7 @@ TEST( ConcurrentBatchSmoother, removeFactors_topology_2 )
NonlinearFactorGraph actualGraph = smoother.getFactors();
NonlinearFactorGraph expectedGraph;
expectedGraph.emplace_shared<PriorFactor<Pose3> >(1, poseInitial, noisePrior);
expectedGraph.addPrior(1, poseInitial, noisePrior);
expectedGraph.emplace_shared<BetweenFactor<Pose3> >(1, 2, poseOdometry, noiseOdometery);
expectedGraph.emplace_shared<BetweenFactor<Pose3> >(2, 3, poseOdometry, noiseOdometery);
expectedGraph.emplace_shared<BetweenFactor<Pose3> >(3, 4, poseOdometry, noiseOdometery);
@ -694,8 +694,8 @@ TEST( ConcurrentBatchSmoother, removeFactors_topology_3 )
// Add some factors to the Smoother
NonlinearFactorGraph newFactors;
newFactors.push_back(PriorFactor<Pose3>(1, poseInitial, noisePrior));
newFactors.push_back(PriorFactor<Pose3>(1, poseInitial, noisePrior));
newFactors.addPrior(1, poseInitial, noisePrior);
newFactors.addPrior(1, poseInitial, noisePrior);
newFactors.push_back(BetweenFactor<Pose3>(1, 2, poseOdometry, noiseOdometery));
newFactors.push_back(BetweenFactor<Pose3>(2, 3, poseOdometry, noiseOdometery));
newFactors.push_back(BetweenFactor<Pose3>(3, 4, poseOdometry, noiseOdometery));
@ -724,7 +724,7 @@ TEST( ConcurrentBatchSmoother, removeFactors_topology_3 )
NonlinearFactorGraph expectedGraph;
// we should add an empty one, so that the ordering and labeling of the factors is preserved
expectedGraph.push_back(NonlinearFactor::shared_ptr());
expectedGraph.emplace_shared<PriorFactor<Pose3> >(1, poseInitial, noisePrior);
expectedGraph.addPrior(1, poseInitial, noisePrior);
expectedGraph.emplace_shared<BetweenFactor<Pose3> >(1, 2, poseOdometry, noiseOdometery);
expectedGraph.emplace_shared<BetweenFactor<Pose3> >(2, 3, poseOdometry, noiseOdometery);
expectedGraph.emplace_shared<BetweenFactor<Pose3> >(3, 4, poseOdometry, noiseOdometery);
@ -744,7 +744,7 @@ TEST( ConcurrentBatchSmoother, removeFactors_values )
// Add some factors to the Smoother
NonlinearFactorGraph newFactors;
newFactors.emplace_shared<PriorFactor<Pose3> >(1, poseInitial, noisePrior);
newFactors.addPrior(1, poseInitial, noisePrior);
newFactors.emplace_shared<BetweenFactor<Pose3> >(1, 2, poseOdometry, noiseOdometery);
newFactors.emplace_shared<BetweenFactor<Pose3> >(2, 3, poseOdometry, noiseOdometery);
newFactors.emplace_shared<BetweenFactor<Pose3> >(3, 4, poseOdometry, noiseOdometery);
@ -774,7 +774,7 @@ TEST( ConcurrentBatchSmoother, removeFactors_values )
// note: factors are removed before the optimization
NonlinearFactorGraph expectedGraph;
expectedGraph.emplace_shared<PriorFactor<Pose3> >(1, poseInitial, noisePrior);
expectedGraph.addPrior(1, poseInitial, noisePrior);
expectedGraph.emplace_shared<BetweenFactor<Pose3> >(1, 2, poseOdometry, noiseOdometery);
expectedGraph.emplace_shared<BetweenFactor<Pose3> >(2, 3, poseOdometry, noiseOdometery);
expectedGraph.emplace_shared<BetweenFactor<Pose3> >(3, 4, poseOdometry, noiseOdometery);

38
gtsam_unstable/nonlinear/tests/testConcurrentIncrementalFilter.cpp
View File

@ -17,7 +17,7 @@
*/
#include <gtsam_unstable/nonlinear/ConcurrentIncrementalFilter.h>
#include <gtsam/slam/PriorFactor.h>
#include <gtsam/nonlinear/PriorFactor.h>
#include <gtsam/slam/BetweenFactor.h>
#include <gtsam/nonlinear/ISAM2.h>
#include <gtsam/nonlinear/LevenbergMarquardtOptimizer.h>
@ -153,7 +153,7 @@ TEST( ConcurrentIncrementalFilter, getFactors )
// Add some factors to the filter
NonlinearFactorGraph newFactors1;
newFactors1.push_back(PriorFactor<Pose3>(1, poseInitial, noisePrior));
newFactors1.addPrior(1, poseInitial, noisePrior);
newFactors1.push_back(BetweenFactor<Pose3>(1, 2, poseOdometry, noiseOdometery));
Values newValues1;
newValues1.insert(1, Pose3());
@ -203,7 +203,7 @@ TEST( ConcurrentIncrementalFilter, getLinearizationPoint )
// Add some factors to the filter
NonlinearFactorGraph newFactors1;
newFactors1.push_back(PriorFactor<Pose3>(1, poseInitial, noisePrior));
newFactors1.addPrior(1, poseInitial, noisePrior);
newFactors1.push_back(BetweenFactor<Pose3>(1, 2, poseOdometry, noiseOdometery));
Values newValues1;
newValues1.insert(1, Pose3());
@ -259,7 +259,7 @@ TEST( ConcurrentIncrementalFilter, calculateEstimate )
// Add some factors to the filter
NonlinearFactorGraph newFactors2;
newFactors2.push_back(PriorFactor<Pose3>(1, poseInitial, noisePrior));
newFactors2.addPrior(1, poseInitial, noisePrior);
newFactors2.push_back(BetweenFactor<Pose3>(1, 2, poseOdometry, noiseOdometery));
Values newValues2;
newValues2.insert(1, Pose3().compose(poseError));
@ -343,7 +343,7 @@ TEST( ConcurrentIncrementalFilter, update_multiple )
// Add some factors to the filter
NonlinearFactorGraph newFactors2;
newFactors2.push_back(PriorFactor<Pose3>(1, poseInitial, noisePrior));
newFactors2.addPrior(1, poseInitial, noisePrior);
newFactors2.push_back(BetweenFactor<Pose3>(1, 2, poseOdometry, noiseOdometery));
Values newValues2;
newValues2.insert(1, Pose3().compose(poseError));
@ -393,7 +393,7 @@ TEST( ConcurrentIncrementalFilter, update_and_marginalize_1 )
// Add some factors to the filter
NonlinearFactorGraph newFactors;
newFactors.push_back(PriorFactor<Pose3>(1, poseInitial, noisePrior));
newFactors.addPrior(1, poseInitial, noisePrior);
newFactors.push_back(BetweenFactor<Pose3>(1, 2, poseOdometry, noiseOdometery));
newFactors.push_back(BetweenFactor<Pose3>(2, 3, poseOdometry, noiseOdometery));
newFactors.push_back(BetweenFactor<Pose3>(3, 4, poseOdometry, noiseOdometery));
@ -423,7 +423,7 @@ TEST( ConcurrentIncrementalFilter, update_and_marginalize_1 )
// ----------------------------------------------------------------------------------------------
NonlinearFactorGraph partialGraph;
partialGraph.emplace_shared<PriorFactor<Pose3> >(1, poseInitial, noisePrior);
partialGraph.addPrior(1, poseInitial, noisePrior);
partialGraph.emplace_shared<BetweenFactor<Pose3> >(1, 2, poseOdometry, noiseOdometery);
partialGraph.emplace_shared<BetweenFactor<Pose3> >(2, 3, poseOdometry, noiseOdometery);
@ -476,7 +476,7 @@ TEST( ConcurrentIncrementalFilter, update_and_marginalize_2 )
// Add some factors to the filter
NonlinearFactorGraph newFactors;
newFactors.push_back(PriorFactor<Pose3>(1, poseInitial, noisePrior));
newFactors.addPrior(1, poseInitial, noisePrior);
newFactors.push_back(BetweenFactor<Pose3>(1, 2, poseOdometry, noiseOdometery));
newFactors.push_back(BetweenFactor<Pose3>(2, 3, poseOdometry, noiseOdometery));
newFactors.push_back(BetweenFactor<Pose3>(3, 4, poseOdometry, noiseOdometery));
@ -507,7 +507,7 @@ TEST( ConcurrentIncrementalFilter, update_and_marginalize_2 )
// ----------------------------------------------------------------------------------------------
NonlinearFactorGraph partialGraph;
partialGraph.emplace_shared<PriorFactor<Pose3> >(1, poseInitial, noisePrior);
partialGraph.addPrior(1, poseInitial, noisePrior);
partialGraph.emplace_shared<BetweenFactor<Pose3> >(1, 2, poseOdometry, noiseOdometery);
partialGraph.emplace_shared<BetweenFactor<Pose3> >(2, 3, poseOdometry, noiseOdometery);
@ -605,7 +605,7 @@ TEST( ConcurrentIncrementalFilter, synchronize_1 )
// Insert factors into the filter, but do not marginalize out any variables.
// The synchronization should still be empty
NonlinearFactorGraph newFactors;
newFactors.push_back(PriorFactor<Pose3>(1, poseInitial, noisePrior));
newFactors.addPrior(1, poseInitial, noisePrior);
newFactors.push_back(BetweenFactor<Pose3>(1, 2, poseOdometry, noiseOdometery));
Values newValues;
newValues.insert(1, Pose3().compose(poseError));
@ -1192,7 +1192,7 @@ TEST( ConcurrentIncrementalFilter, removeFactors_topology_1 )
// Add some factors to the filter
NonlinearFactorGraph newFactors;
newFactors.push_back(PriorFactor<Pose3>(1, poseInitial, noisePrior));
newFactors.addPrior(1, poseInitial, noisePrior);
newFactors.push_back(BetweenFactor<Pose3>(1, 2, poseOdometry, noiseOdometery));
newFactors.push_back(BetweenFactor<Pose3>(2, 3, poseOdometry, noiseOdometery));
newFactors.push_back(BetweenFactor<Pose3>(3, 4, poseOdometry, noiseOdometery));
@ -1224,7 +1224,7 @@ TEST( ConcurrentIncrementalFilter, removeFactors_topology_1 )
NonlinearFactorGraph actualGraph = filter.getFactors();
NonlinearFactorGraph expectedGraph;
expectedGraph.emplace_shared<PriorFactor<Pose3> >(1, poseInitial, noisePrior);
expectedGraph.addPrior(1, poseInitial, noisePrior);
// we removed this one: expectedGraph.push_back(BetweenFactor<Pose3>(1, 2, poseOdometry, noiseOdometery))
// we should add an empty one, so that the ordering and labeling of the factors is preserved
expectedGraph.push_back(NonlinearFactor::shared_ptr());
@ -1251,7 +1251,7 @@ TEST( ConcurrentIncrementalFilter, removeFactors_topology_2 )
// Add some factors to the filter
NonlinearFactorGraph newFactors;
newFactors.push_back(PriorFactor<Pose3>(1, poseInitial, noisePrior));
newFactors.addPrior(1, poseInitial, noisePrior);
newFactors.push_back(BetweenFactor<Pose3>(1, 2, poseOdometry, noiseOdometery));
newFactors.push_back(BetweenFactor<Pose3>(2, 3, poseOdometry, noiseOdometery));
newFactors.push_back(BetweenFactor<Pose3>(3, 4, poseOdometry, noiseOdometery));
@ -1282,7 +1282,7 @@ TEST( ConcurrentIncrementalFilter, removeFactors_topology_2 )
NonlinearFactorGraph actualGraph = filter.getFactors();
NonlinearFactorGraph expectedGraph;
expectedGraph.emplace_shared<PriorFactor<Pose3> >(1, poseInitial, noisePrior);
expectedGraph.addPrior(1, poseInitial, noisePrior);
expectedGraph.emplace_shared<BetweenFactor<Pose3> >(1, 2, poseOdometry, noiseOdometery);
expectedGraph.emplace_shared<BetweenFactor<Pose3> >(2, 3, poseOdometry, noiseOdometery);
expectedGraph.emplace_shared<BetweenFactor<Pose3> >(3, 4, poseOdometry, noiseOdometery);
@ -1310,8 +1310,8 @@ TEST( ConcurrentIncrementalFilter, removeFactors_topology_3 )
// Add some factors to the filter
NonlinearFactorGraph newFactors;
newFactors.push_back(PriorFactor<Pose3>(1, poseInitial, noisePrior));
newFactors.push_back(PriorFactor<Pose3>(1, poseInitial, noisePrior));
newFactors.addPrior(1, poseInitial, noisePrior);
newFactors.addPrior(1, poseInitial, noisePrior);
newFactors.push_back(BetweenFactor<Pose3>(1, 2, poseOdometry, noiseOdometery));
newFactors.push_back(BetweenFactor<Pose3>(2, 3, poseOdometry, noiseOdometery));
newFactors.push_back(BetweenFactor<Pose3>(3, 4, poseOdometry, noiseOdometery));
@ -1343,7 +1343,7 @@ TEST( ConcurrentIncrementalFilter, removeFactors_topology_3 )
NonlinearFactorGraph expectedGraph;
// we should add an empty one, so that the ordering and labeling of the factors is preserved
expectedGraph.push_back(NonlinearFactor::shared_ptr());
expectedGraph.emplace_shared<PriorFactor<Pose3> >(1, poseInitial, noisePrior);
expectedGraph.addPrior(1, poseInitial, noisePrior);
expectedGraph.emplace_shared<BetweenFactor<Pose3> >(1, 2, poseOdometry, noiseOdometery);
expectedGraph.emplace_shared<BetweenFactor<Pose3> >(2, 3, poseOdometry, noiseOdometery);
expectedGraph.emplace_shared<BetweenFactor<Pose3> >(3, 4, poseOdometry, noiseOdometery);
@ -1367,7 +1367,7 @@ TEST( ConcurrentIncrementalFilter, removeFactors_values )
// Add some factors to the filter
NonlinearFactorGraph newFactors;
newFactors.push_back(PriorFactor<Pose3>(1, poseInitial, noisePrior));
newFactors.addPrior(1, poseInitial, noisePrior);
newFactors.push_back(BetweenFactor<Pose3>(1, 2, poseOdometry, noiseOdometery));
newFactors.push_back(BetweenFactor<Pose3>(2, 3, poseOdometry, noiseOdometery));
newFactors.push_back(BetweenFactor<Pose3>(3, 4, poseOdometry, noiseOdometery));
@ -1399,7 +1399,7 @@ TEST( ConcurrentIncrementalFilter, removeFactors_values )
Values actualValues = filter.getLinearizationPoint();
NonlinearFactorGraph expectedGraph;
expectedGraph.emplace_shared<PriorFactor<Pose3> >(1, poseInitial, noisePrior);
expectedGraph.addPrior(1, poseInitial, noisePrior);
expectedGraph.emplace_shared<BetweenFactor<Pose3> >(1, 2, poseOdometry, noiseOdometery);
expectedGraph.emplace_shared<BetweenFactor<Pose3> >(2, 3, poseOdometry, noiseOdometery);
expectedGraph.emplace_shared<BetweenFactor<Pose3> >(3, 4, poseOdometry, noiseOdometery);

12
gtsam_unstable/nonlinear/tests/testConcurrentIncrementalSmootherDL.cpp
View File

@ -17,7 +17,7 @@
*/
#include <gtsam_unstable/nonlinear/ConcurrentIncrementalSmoother.h>
#include <gtsam/slam/PriorFactor.h>
#include <gtsam/nonlinear/PriorFactor.h>
#include <gtsam/slam/BetweenFactor.h>
#include <gtsam/nonlinear/ISAM2.h>
#include <gtsam/nonlinear/LevenbergMarquardtOptimizer.h>
@ -115,7 +115,7 @@ TEST( ConcurrentIncrementalSmootherDL, getFactors )
// Add some factors to the smoother
NonlinearFactorGraph newFactors1;
newFactors1.push_back(PriorFactor<Pose3>(1, poseInitial, noisePrior));
newFactors1.addPrior(1, poseInitial, noisePrior);
newFactors1.push_back(BetweenFactor<Pose3>(1, 2, poseOdometry, noiseOdometery));
Values newValues1;
newValues1.insert(1, Pose3());
@ -166,7 +166,7 @@ TEST( ConcurrentIncrementalSmootherDL, getLinearizationPoint )
// Add some factors to the smoother
NonlinearFactorGraph newFactors1;
newFactors1.push_back(PriorFactor<Pose3>(1, poseInitial, noisePrior));
newFactors1.addPrior(1, poseInitial, noisePrior);
newFactors1.push_back(BetweenFactor<Pose3>(1, 2, poseOdometry, noiseOdometery));
Values newValues1;
newValues1.insert(1, Pose3());
@ -223,7 +223,7 @@ TEST( ConcurrentIncrementalSmootherDL, calculateEstimate )
// Add some factors to the smoother
NonlinearFactorGraph newFactors2;
newFactors2.push_back(PriorFactor<Pose3>(1, poseInitial, noisePrior));
newFactors2.addPrior(1, poseInitial, noisePrior);
newFactors2.push_back(BetweenFactor<Pose3>(1, 2, poseOdometry, noiseOdometery));
Values newValues2;
newValues2.insert(1, Pose3().compose(poseError));
@ -310,7 +310,7 @@ TEST( ConcurrentIncrementalSmootherDL, update_multiple )
// Add some factors to the smoother
NonlinearFactorGraph newFactors2;
newFactors2.push_back(PriorFactor<Pose3>(1, poseInitial, noisePrior));
newFactors2.addPrior(1, poseInitial, noisePrior);
newFactors2.push_back(BetweenFactor<Pose3>(1, 2, poseOdometry, noiseOdometery));
Values newValues2;
newValues2.insert(1, Pose3().compose(poseError));
@ -545,7 +545,7 @@ TEST( ConcurrentIncrementalSmootherDL, synchronize_3 )
filterSumarization.push_back(LinearContainerFactor(BetweenFactor<Pose3>(1, 2, poseOdometry, noiseOdometery).linearize(filterSeparatorValues), filterSeparatorValues));
smootherFactors.push_back(BetweenFactor<Pose3>(2, 3, poseOdometry, noiseOdometery));
smootherFactors.push_back(BetweenFactor<Pose3>(3, 4, poseOdometry, noiseOdometery));
smootherFactors.push_back(PriorFactor<Pose3>(4, poseInitial, noisePrior));
smootherFactors.addPrior(4, poseInitial, noisePrior);
smootherValues.insert(3, filterSeparatorValues.at<Pose3>(2).compose(poseOdometry).compose(poseError));
smootherValues.insert(4, smootherValues.at<Pose3>(3).compose(poseOdometry).compose(poseError));

16
gtsam_unstable/nonlinear/tests/testConcurrentIncrementalSmootherGN.cpp
View File

@ -17,7 +17,7 @@
*/
#include <gtsam_unstable/nonlinear/ConcurrentIncrementalSmoother.h>
#include <gtsam/slam/PriorFactor.h>
#include <gtsam/nonlinear/PriorFactor.h>
#include <gtsam/slam/BetweenFactor.h>
#include <gtsam/nonlinear/ISAM2.h>
#include <gtsam/nonlinear/LevenbergMarquardtOptimizer.h>
@ -114,7 +114,7 @@ TEST( ConcurrentIncrementalSmootherGN, getFactors )
// Add some factors to the smoother
NonlinearFactorGraph newFactors1;
newFactors1.push_back(PriorFactor<Pose3>(1, poseInitial, noisePrior));
newFactors1.addPrior(1, poseInitial, noisePrior);
newFactors1.push_back(BetweenFactor<Pose3>(1, 2, poseOdometry, noiseOdometery));
Values newValues1;
newValues1.insert(1, Pose3());
@ -165,7 +165,7 @@ TEST( ConcurrentIncrementalSmootherGN, getLinearizationPoint )
// Add some factors to the smoother
NonlinearFactorGraph newFactors1;
newFactors1.push_back(PriorFactor<Pose3>(1, poseInitial, noisePrior));
newFactors1.addPrior(1, poseInitial, noisePrior);
newFactors1.push_back(BetweenFactor<Pose3>(1, 2, poseOdometry, noiseOdometery));
Values newValues1;
newValues1.insert(1, Pose3());
@ -222,7 +222,7 @@ TEST( ConcurrentIncrementalSmootherGN, calculateEstimate )
// Add some factors to the smoother
NonlinearFactorGraph newFactors2;
newFactors2.push_back(PriorFactor<Pose3>(1, poseInitial, noisePrior));
newFactors2.addPrior(1, poseInitial, noisePrior);
newFactors2.push_back(BetweenFactor<Pose3>(1, 2, poseOdometry, noiseOdometery));
Values newValues2;
newValues2.insert(1, Pose3().compose(poseError));
@ -309,7 +309,7 @@ TEST( ConcurrentIncrementalSmootherGN, update_multiple )
// Add some factors to the smoother
NonlinearFactorGraph newFactors2;
newFactors2.push_back(PriorFactor<Pose3>(1, poseInitial, noisePrior));
newFactors2.addPrior(1, poseInitial, noisePrior);
newFactors2.push_back(BetweenFactor<Pose3>(1, 2, poseOdometry, noiseOdometery));
Values newValues2;
newValues2.insert(1, Pose3().compose(poseError));
@ -546,7 +546,7 @@ TEST( ConcurrentIncrementalSmootherGN, synchronize_3 )
filterSumarization.push_back(LinearContainerFactor(BetweenFactor<Pose3>(1, 2, poseOdometry, noiseOdometery).linearize(filterSeparatorValues), filterSeparatorValues));
smootherFactors.push_back(BetweenFactor<Pose3>(2, 3, poseOdometry, noiseOdometery));
smootherFactors.push_back(BetweenFactor<Pose3>(3, 4, poseOdometry, noiseOdometery));
smootherFactors.push_back(PriorFactor<Pose3>(4, poseInitial, noisePrior));
smootherFactors.addPrior(4, poseInitial, noisePrior);
smootherValues.insert(3, filterSeparatorValues.at<Pose3>(2).compose(poseOdometry).compose(poseError));
smootherValues.insert(4, smootherValues.at<Pose3>(3).compose(poseOdometry).compose(poseError));
@ -609,7 +609,7 @@ TEST( ConcurrentIncrementalSmoother, removeFactors_topology_1 )
// Add some factors to the smoother
NonlinearFactorGraph newFactors;
newFactors.push_back(PriorFactor<Pose3>(1, poseInitial, noisePrior));
newFactors.addPrior(1, poseInitial, noisePrior);
newFactors.push_back(BetweenFactor<Pose3>(1, 2, poseOdometry, noiseOdometery));
newFactors.push_back(BetweenFactor<Pose3>(2, 3, poseOdometry, noiseOdometery));
newFactors.push_back(BetweenFactor<Pose3>(3, 4, poseOdometry, noiseOdometery));
@ -639,7 +639,7 @@ TEST( ConcurrentIncrementalSmoother, removeFactors_topology_1 )
actualGraph.print("actual graph: \n");
NonlinearFactorGraph expectedGraph;
expectedGraph.emplace_shared<PriorFactor<Pose3> >(1, poseInitial, noisePrior);
expectedGraph.addPrior(1, poseInitial, noisePrior);
// we removed this one: expectedGraph.push_back(BetweenFactor<Pose3>(1, 2, poseOdometry, noiseOdometery));
// we should add an empty one, so that the ordering and labeling of the factors is preserved
expectedGraph.push_back(NonlinearFactor::shared_ptr());

3
gtsam_unstable/nonlinear/tests/testIncrementalFixedLagSmoother.cpp
View File

@ -18,7 +18,6 @@
#include <gtsam_unstable/nonlinear/IncrementalFixedLagSmoother.h>
#include <gtsam/slam/PriorFactor.h>
#include <gtsam/slam/BetweenFactor.h>
#include <gtsam/geometry/Point2.h>
#include <gtsam/nonlinear/NonlinearFactorGraph.h>
@ -82,7 +81,7 @@ TEST( IncrementalFixedLagSmoother, Example )
Values newValues;
Timestamps newTimestamps;
newFactors.push_back(PriorFactor<Point2>(key0, Point2(0.0, 0.0), odometerNoise));
newFactors.addPrior(key0, Point2(0.0, 0.0), odometerNoise);
newValues.insert(key0, Point2(0.01, 0.01));
newTimestamps[key0] = 0.0;

3
gtsam_unstable/nonlinear/tests/testNonlinearClusterTree.cpp
View File

@ -6,7 +6,6 @@
#include <gtsam_unstable/nonlinear/NonlinearClusterTree.h>
#include <gtsam/sam/BearingRangeFactor.h>
#include <gtsam/slam/PriorFactor.h>
#include <gtsam/slam/BetweenFactor.h>
#include <gtsam/geometry/Pose2.h>
#include <gtsam/geometry/Point2.h>
@ -27,7 +26,7 @@ NonlinearFactorGraph planarSLAMGraph() {
// Prior on pose x1 at the origin.
Pose2 prior(0.0, 0.0, 0.0);
auto priorNoise = noiseModel::Diagonal::Sigmas(Vector3(0.3, 0.3, 0.1));
graph.emplace_shared<PriorFactor<Pose2> >(x1, prior, priorNoise);
graph.addPrior(x1, prior, priorNoise);
// Two odometry factors
Pose2 odometry(2.0, 0.0, 0.0);

2
gtsam_unstable/slam/serialization.cpp
View File

@ -15,7 +15,7 @@
#include <gtsam/slam/BetweenFactor.h>
//#include <gtsam/slam/BoundingConstraint.h>
#include <gtsam/slam/GeneralSFMFactor.h>
#include <gtsam/slam/PriorFactor.h>
#include <gtsam/nonlinear/PriorFactor.h>
#include <gtsam/slam/ProjectionFactor.h>
#include <gtsam/sam/RangeFactor.h>
#include <gtsam/slam/StereoFactor.h>

5
gtsam_unstable/slam/tests/testBetweenFactorEM.cpp
View File

@ -13,7 +13,6 @@
#include <gtsam/base/numericalDerivative.h>
#include <gtsam/slam/BetweenFactor.h>
#include <gtsam/slam/PriorFactor.h>
using namespace std;
@ -280,8 +279,8 @@ TEST (BetweenFactorEM, updateNoiseModel ) {
SharedGaussian model = SharedGaussian(noiseModel::Isotropic::Sigma(3, 1e2));
NonlinearFactorGraph graph;
graph.push_back(gtsam::PriorFactor<Pose2>(key1, p1, model));
graph.push_back(gtsam::PriorFactor<Pose2>(key2, p2, model));
graph.addPrior(key1, p1, model);
graph.addPrior(key2, p2, model);
f.updateNoiseModels(values, graph);

1
gtsam_unstable/slam/tests/testMultiProjectionFactor.cpp
View File

@ -16,7 +16,6 @@
* @date Nov 2009
*/
#include <gtsam/slam/PriorFactor.h>
#include <gtsam/slam/BetweenFactor.h>
#include <gtsam/slam/ProjectionFactor.h>
#include <gtsam_unstable/slam/MultiProjectionFactor.h>

3
gtsam_unstable/slam/tests/testSerialization.cpp
View File

@ -15,7 +15,6 @@
#include <gtsam/geometry/Pose2.h>
#include <gtsam/geometry/Pose3.h>
#include <gtsam/slam/PriorFactor.h>
#include <gtsam/slam/BetweenFactor.h>
#include <gtsam/sam/BearingRangeFactor.h>
@ -48,7 +47,7 @@ Values exampleValues() {
NonlinearFactorGraph exampleGraph() {
NonlinearFactorGraph graph;
graph.add(PriorFactor<Pose2>(234, Pose2(1.0, 2.0, 0.3), noiseModel::Diagonal::Sigmas(Vector::Ones(3))));
graph.addPrior(234, Pose2(1.0, 2.0, 0.3), noiseModel::Diagonal::Sigmas(Vector::Ones(3)));
graph.add(BetweenFactor<Pose2>(234, 567, Pose2(1.0, 2.0, 0.3), noiseModel::Diagonal::Sigmas(Vector::Ones(3))));
graph.add(BearingRangeFactor<Pose2,Point2>(234, 567, Rot2::fromAngle(0.3), 2.0, noiseModel::Diagonal::Sigmas(Vector::Ones(2))));
return graph;

5
gtsam_unstable/slam/tests/testSmartRangeFactor.cpp
View File

@ -19,7 +19,6 @@
#include <gtsam_unstable/slam/SmartRangeFactor.h>
#include <gtsam/nonlinear/NonlinearFactorGraph.h>
#include <gtsam/nonlinear/LevenbergMarquardtOptimizer.h>
#include <gtsam/slam/PriorFactor.h>
#include <CppUnitLite/TestHarness.h>
using namespace std;
@ -116,8 +115,8 @@ TEST( SmartRangeFactor, optimization ) {
graph.push_back(f);
const noiseModel::Base::shared_ptr //
priorNoise = noiseModel::Diagonal::Sigmas(Vector3(1, 1, M_PI));
graph.emplace_shared<PriorFactor<Pose2> >(1, pose1, priorNoise);
graph.emplace_shared<PriorFactor<Pose2> >(2, pose2, priorNoise);
graph.addPrior(1, pose1, priorNoise);
graph.addPrior(2, pose2, priorNoise);
// Try optimizing
LevenbergMarquardtParams params;

8
gtsam_unstable/slam/tests/testSmartStereoFactor_iSAM2.cpp
View File

@ -198,9 +198,7 @@ TEST(testISAM2SmartFactor, Stereo_Batch) {
// prior, for the first keyframe:
if (kf_id == 0) {
const auto prior = boost::make_shared<PriorFactor<Pose3>>(
X(kf_id), Pose3::identity(), priorPoseNoise);
batch_graph.push_back(prior);
batch_graph.addPrior(X(kf_id), Pose3::identity(), priorPoseNoise);
}
batch_values.insert(X(kf_id), Pose3::identity());
@ -309,9 +307,7 @@ TEST(testISAM2SmartFactor, Stereo_iSAM2) {
// prior, for the first keyframe:
if (kf_id == 0) {
const auto prior = boost::make_shared<PriorFactor<Pose3>>(
X(kf_id), Pose3::identity(), priorPoseNoise);
newFactors.push_back(prior);
newFactors.addPrior(X(kf_id), Pose3::identity(), priorPoseNoise);
}
// 2) Run iSAM2:

32
gtsam_unstable/slam/tests/testSmartStereoProjectionPoseFactor.cpp
View File

@ -335,8 +335,8 @@ TEST( SmartStereoProjectionPoseFactor, 3poses_smart_projection_factor ) {
graph.push_back(smartFactor1);
graph.push_back(smartFactor2);
graph.push_back(smartFactor3);
graph.emplace_shared<PriorFactor<Pose3> >(x1, pose1, noisePrior);
graph.emplace_shared<PriorFactor<Pose3> >(x2, pose2, noisePrior);
graph.addPrior(x1, pose1, noisePrior);
graph.addPrior(x2, pose2, noisePrior);
// Pose3 noise_pose = Pose3(Rot3::Ypr(-M_PI/10, 0., -M_PI/10), Point3(0.5,0.1,0.3)); // noise from regular projection factor test below
Pose3 noise_pose = Pose3(Rot3::Ypr(-M_PI / 100, 0., -M_PI / 100),
@ -396,8 +396,8 @@ TEST( SmartStereoProjectionPoseFactor, 3poses_smart_projection_factor ) {
// add factors
NonlinearFactorGraph graph2;
graph2.push_back(PriorFactor<Pose3>(x1, pose1, noisePrior));
graph2.push_back(PriorFactor<Pose3>(x2, pose2, noisePrior));
graph2.addPrior(x1, pose1, noisePrior);
graph2.addPrior(x2, pose2, noisePrior);
typedef GenericStereoFactor<Pose3, Point3> ProjectionFactor;
@ -477,8 +477,8 @@ TEST( SmartStereoProjectionPoseFactor, body_P_sensor ) {
graph.push_back(smartFactor1);
graph.push_back(smartFactor2);
graph.push_back(smartFactor3);
graph.push_back(PriorFactor<Pose3>(x1, pose1, noisePrior));
graph.push_back(PriorFactor<Pose3>(x2, pose2, noisePrior));
graph.addPrior(x1, pose1, noisePrior);
graph.addPrior(x2, pose2, noisePrior);
// Pose3 noise_pose = Pose3(Rot3::Ypr(-M_PI/10, 0., -M_PI/10), Point3(0.5,0.1,0.3)); // noise from regular projection factor test below
Pose3 noise_pose = Pose3(Rot3::Ypr(-M_PI / 100, 0., -M_PI / 100),
@ -586,8 +586,8 @@ TEST( SmartStereoProjectionPoseFactor, body_P_sensor_monocular ){
graph.push_back(smartFactor1);
graph.push_back(smartFactor2);
graph.push_back(smartFactor3);
graph.push_back(PriorFactor<Pose3>(x1, bodyPose1, noisePrior));
graph.push_back(PriorFactor<Pose3>(x2, bodyPose2, noisePrior));
graph.addPrior(x1, bodyPose1, noisePrior);
graph.addPrior(x2, bodyPose2, noisePrior);
// Check errors at ground truth poses
Values gtValues;
@ -660,8 +660,8 @@ TEST( SmartStereoProjectionPoseFactor, jacobianSVD ) {
graph.push_back(smartFactor1);
graph.push_back(smartFactor2);
graph.push_back(smartFactor3);
graph.emplace_shared<PriorFactor<Pose3> >(x1, pose1, noisePrior);
graph.emplace_shared<PriorFactor<Pose3> >(x2, pose2, noisePrior);
graph.addPrior(x1, pose1, noisePrior);
graph.addPrior(x2, pose2, noisePrior);
// Pose3 noise_pose = Pose3(Rot3::Ypr(-M_PI/10, 0., -M_PI/10), Point3(0.5,0.1,0.3)); // noise from regular projection factor test below
Pose3 noise_pose = Pose3(Rot3::Ypr(-M_PI / 100, 0., -M_PI / 100),
@ -732,8 +732,8 @@ TEST( SmartStereoProjectionPoseFactor, jacobianSVDwithMissingValues ) {
graph.push_back(smartFactor1);
graph.push_back(smartFactor2);
graph.push_back(smartFactor3);
graph.push_back(PriorFactor<Pose3>(x1, pose1, noisePrior));
graph.push_back(PriorFactor<Pose3>(x2, pose2, noisePrior));
graph.addPrior(x1, pose1, noisePrior);
graph.addPrior(x2, pose2, noisePrior);
// Pose3 noise_pose = Pose3(Rot3::Ypr(-M_PI/10, 0., -M_PI/10), Point3(0.5,0.1,0.3)); // noise from regular projection factor test below
Pose3 noise_pose = Pose3(Rot3::Ypr(-M_PI / 100, 0., -M_PI / 100),
@ -801,8 +801,8 @@ TEST( SmartStereoProjectionPoseFactor, landmarkDistance ) {
graph.push_back(smartFactor1);
graph.push_back(smartFactor2);
graph.push_back(smartFactor3);
graph.emplace_shared<PriorFactor<Pose3> >(x1, pose1, noisePrior);
graph.emplace_shared<PriorFactor<Pose3> >(x2, pose2, noisePrior);
graph.addPrior(x1, pose1, noisePrior);
graph.addPrior(x2, pose2, noisePrior);
// Pose3 noise_pose = Pose3(Rot3::Ypr(-M_PI/10, 0., -M_PI/10), Point3(0.5,0.1,0.3)); // noise from regular projection factor test below
Pose3 noise_pose = Pose3(Rot3::Ypr(-M_PI / 100, 0., -M_PI / 100),
@ -883,8 +883,8 @@ TEST( SmartStereoProjectionPoseFactor, dynamicOutlierRejection ) {
graph.push_back(smartFactor2);
graph.push_back(smartFactor3);
graph.push_back(smartFactor4);
graph.emplace_shared<PriorFactor<Pose3> >(x1, pose1, noisePrior);
graph.emplace_shared<PriorFactor<Pose3> >(x2, pose2, noisePrior);
graph.addPrior(x1, pose1, noisePrior);
graph.addPrior(x2, pose2, noisePrior);
Pose3 noise_pose = Pose3(Rot3::Ypr(-M_PI / 100, 0., -M_PI / 100),
Point3(0.1, 0.1, 0.1)); // smaller noise

2
tests/testExpressionFactor.cpp
View File

@ -20,7 +20,7 @@
#include <gtsam/slam/expressions.h>
#include <gtsam/slam/GeneralSFMFactor.h>
#include <gtsam/slam/ProjectionFactor.h>
#include <gtsam/slam/PriorFactor.h>
#include <gtsam/nonlinear/PriorFactor.h>
#include <gtsam/nonlinear/expressionTesting.h>
#include <gtsam/nonlinear/ExpressionFactor.h>
#include <gtsam/nonlinear/NonlinearFactorGraph.h>

2
tests/testExtendedKalmanFilter.cpp
View File

@ -14,7 +14,7 @@
* @author Stephen Williams
*/
#include <gtsam/slam/PriorFactor.h>
#include <gtsam/nonlinear/PriorFactor.h>
#include <gtsam/slam/BetweenFactor.h>
#include <gtsam/nonlinear/ExtendedKalmanFilter-inl.h>
#include <gtsam/nonlinear/NonlinearFactor.h>

3
tests/testGaussianFactorGraphB.cpp
View File

@ -401,7 +401,6 @@ TEST(GaussianFactorGraph, hasConstraints)
#include <gtsam/slam/ProjectionFactor.h>
#include <gtsam/geometry/Pose3.h>
#include <gtsam/slam/PriorFactor.h>
#include <gtsam/sam/RangeFactor.h>
/* ************************************************************************* */
@ -438,7 +437,7 @@ TEST( GaussianFactorGraph, conditional_sigma_failure) {
initValues.insert(l41, Point3(1.61051523, 6.7373052, -0.231582751) );
NonlinearFactorGraph factors;
factors += PriorFactor<Pose3>(xC1,
factors.addPrior(xC1,
Pose3(Rot3(
-1., 0.0, 1.2246468e-16,
0.0, 1., 0.0,

13
tests/testGaussianISAM2.cpp
View File

@ -7,7 +7,6 @@
#include <gtsam/nonlinear/ISAM2.h>
#include <tests/smallExample.h>
#include <gtsam/slam/PriorFactor.h>
#include <gtsam/slam/BetweenFactor.h>
#include <gtsam/sam/BearingRangeFactor.h>
#include <gtsam/geometry/Point2.h>
@ -63,7 +62,7 @@ ISAM2 createSlamlikeISAM2(
// Add a prior at time 0 and update isam
{
NonlinearFactorGraph newfactors;
newfactors += PriorFactor<Pose2>(0, Pose2(0.0, 0.0, 0.0), odoNoise);
newfactors.addPrior(0, Pose2(0.0, 0.0, 0.0), odoNoise);
fullgraph.push_back(newfactors);
Values init;
@ -499,7 +498,7 @@ TEST(ISAM2, constrained_ordering)
// Add a prior at time 0 and update isam
{
NonlinearFactorGraph newfactors;
newfactors += PriorFactor<Pose2>(0, Pose2(0.0, 0.0, 0.0), odoNoise);
newfactors.addPrior(0, Pose2(0.0, 0.0, 0.0), odoNoise);
fullgraph.push_back(newfactors);
Values init;
@ -669,7 +668,7 @@ namespace {
TEST(ISAM2, marginalizeLeaves1) {
ISAM2 isam;
NonlinearFactorGraph factors;
factors += PriorFactor<double>(0, 0.0, model);
factors.addPrior(0, 0.0, model);
factors += BetweenFactor<double>(0, 1, 0.0, model);
factors += BetweenFactor<double>(1, 2, 0.0, model);
@ -696,7 +695,7 @@ TEST(ISAM2, marginalizeLeaves2) {
ISAM2 isam;
NonlinearFactorGraph factors;
factors += PriorFactor<double>(0, 0.0, model);
factors.addPrior(0, 0.0, model);
factors += BetweenFactor<double>(0, 1, 0.0, model);
factors += BetweenFactor<double>(1, 2, 0.0, model);
@ -726,7 +725,7 @@ TEST(ISAM2, marginalizeLeaves3) {
ISAM2 isam;
NonlinearFactorGraph factors;
factors += PriorFactor<double>(0, 0.0, model);
factors.addPrior(0, 0.0, model);
factors += BetweenFactor<double>(0, 1, 0.0, model);
factors += BetweenFactor<double>(1, 2, 0.0, model);
@ -765,7 +764,7 @@ TEST(ISAM2, marginalizeLeaves4) {
ISAM2 isam;
NonlinearFactorGraph factors;
factors += PriorFactor<double>(0, 0.0, model);
factors.addPrior(0, 0.0, model);
factors += BetweenFactor<double>(0, 2, 0.0, model);
factors += BetweenFactor<double>(1, 2, 0.0, model);

2
tests/testGaussianJunctionTreeB.cpp
View File

@ -18,7 +18,7 @@
#include <tests/smallExample.h>
#include <gtsam/sam/BearingRangeFactor.h>
#include <gtsam/slam/BetweenFactor.h>
#include <gtsam/slam/PriorFactor.h>
#include <gtsam/nonlinear/PriorFactor.h>
#include <gtsam/geometry/Pose2.h>
#include <gtsam/nonlinear/NonlinearFactorGraph.h>
#include <gtsam/nonlinear/Values.h>

3
tests/testGradientDescentOptimizer.cpp
View File

@ -12,7 +12,6 @@
* @date Jun 11, 2012
*/
#include <gtsam/slam/PriorFactor.h>
#include <gtsam/slam/BetweenFactor.h>
#include <gtsam/nonlinear/NonlinearConjugateGradientOptimizer.h>
#include <gtsam/nonlinear/NonlinearFactorGraph.h>
@ -38,7 +37,7 @@ boost::tuple<NonlinearFactorGraph, Values> generateProblem() {
Pose2 priorMean(0.0, 0.0, 0.0); // prior at origin
SharedDiagonal priorNoise = noiseModel::Diagonal::Sigmas(
Vector3(0.3, 0.3, 0.1));
graph += PriorFactor<Pose2>(1, priorMean, priorNoise);
graph.addPrior(1, priorMean, priorNoise);
// 2b. Add odometry factors
SharedDiagonal odometryNoise = noiseModel::Diagonal::Sigmas(

3
tests/testIterative.cpp
View File

@ -16,7 +16,6 @@
**/
#include <tests/smallExample.h>
#include <gtsam/slam/PriorFactor.h>
#include <gtsam/slam/BetweenFactor.h>
#include <gtsam/nonlinear/NonlinearEquality.h>
#include <gtsam/inference/Symbol.h>
@ -117,7 +116,7 @@ TEST( Iterative, conjugateGradientDescent_soft_constraint )
config.insert(X(2), Pose2(1.5,0.,0.));
NonlinearFactorGraph graph;
graph += PriorFactor<Pose2>(X(1), Pose2(0.,0.,0.), noiseModel::Isotropic::Sigma(3, 1e-10));
graph.addPrior(X(1), Pose2(0.,0.,0.), noiseModel::Isotropic::Sigma(3, 1e-10));
graph += BetweenFactor<Pose2>(X(1),X(2), Pose2(1.,0.,0.), noiseModel::Isotropic::Sigma(3, 1));
boost::shared_ptr<GaussianFactorGraph> fg = graph.linearize(config);

5
tests/testMarginals.cpp
View File

@ -29,7 +29,6 @@
#include <gtsam/linear/NoiseModel.h>
// add in headers for specific factors
#include <gtsam/slam/PriorFactor.h>
#include <gtsam/slam/BetweenFactor.h>
#include <gtsam/sam/BearingRangeFactor.h>
@ -53,7 +52,7 @@ TEST(Marginals, planarSLAMmarginals) {
// gaussian for prior
SharedDiagonal priorNoise = noiseModel::Diagonal::Sigmas(Vector3(0.3, 0.3, 0.1));
Pose2 priorMean(0.0, 0.0, 0.0); // prior at origin
graph += PriorFactor<Pose2>(x1, priorMean, priorNoise); // add the factor to the graph
graph.addPrior(x1, priorMean, priorNoise); // add the factor to the graph
/* add odometry */
// general noisemodel for odometry
@ -195,7 +194,7 @@ TEST(Marginals, planarSLAMmarginals) {
/* ************************************************************************* */
TEST(Marginals, order) {
NonlinearFactorGraph fg;
fg += PriorFactor<Pose2>(0, Pose2(), noiseModel::Unit::Create(3));
fg.addPrior(0, Pose2(), noiseModel::Unit::Create(3));
fg += BetweenFactor<Pose2>(0, 1, Pose2(1,0,0), noiseModel::Unit::Create(3));
fg += BetweenFactor<Pose2>(1, 2, Pose2(1,0,0), noiseModel::Unit::Create(3));
fg += BetweenFactor<Pose2>(2, 3, Pose2(1,0,0), noiseModel::Unit::Create(3));

2
tests/testNonlinearEquality.cpp
View File

@ -16,7 +16,7 @@
#include <tests/simulated2DConstraints.h>
#include <gtsam/slam/PriorFactor.h>
#include <gtsam/nonlinear/PriorFactor.h>
#include <gtsam/slam/ProjectionFactor.h>
#include <gtsam/nonlinear/NonlinearEquality.h>
#include <gtsam/nonlinear/NonlinearFactorGraph.h>

48
tests/testNonlinearFactorGraph.cpp
View File

@ -25,8 +25,8 @@
#include <gtsam/symbolic/SymbolicFactorGraph.h>
#include <gtsam/nonlinear/NonlinearFactorGraph.h>
#include <gtsam/geometry/Pose2.h>
#include <gtsam/geometry/Pose3.h>
#include <gtsam/sam/RangeFactor.h>
#include <gtsam/slam/PriorFactor.h>
#include <gtsam/slam/BetweenFactor.h>
#include <CppUnitLite/TestHarness.h>
@ -216,8 +216,8 @@ TEST(testNonlinearFactorGraph, eliminate) {
// Priors
auto prior = noiseModel::Isotropic::Sigma(3, 1);
graph.add(PriorFactor<Pose2>(11, T11, prior));
graph.add(PriorFactor<Pose2>(21, T21, prior));
graph.addPrior(11, T11, prior);
graph.addPrior(21, T21, prior);
// Odometry
auto model = noiseModel::Diagonal::Sigmas(Vector3(0.01, 0.01, 0.3));
@ -243,6 +243,48 @@ TEST(testNonlinearFactorGraph, eliminate) {
}
/* ************************************************************************* */
TEST(testNonlinearFactorGraph, addPrior) {
Key k(0);
// Factor graph.
auto graph = NonlinearFactorGraph();
// Add a prior factor for key k.
auto model_double = noiseModel::Isotropic::Sigma(1, 1);
graph.addPrior<double>(k, 10, model_double);
// Assert the graph has 0 error with the correct values.
Values values;
values.insert(k, 10.0);
EXPECT_DOUBLES_EQUAL(0, graph.error(values), 1e-16);
// Assert the graph has some error with incorrect values.
values.clear();
values.insert(k, 11.0);
EXPECT(0 != graph.error(values));
// Clear the factor graph and values.
values.clear();
graph.erase(graph.begin(), graph.end());
// Add a Pose3 prior to the factor graph. Use a gaussian noise model by
// providing the covariance matrix.
Eigen::DiagonalMatrix<double, 6, 6> covariance_pose3;
covariance_pose3.setIdentity();
Pose3 pose{Rot3(), Point3(0, 0, 0)};
graph.addPrior(k, pose, covariance_pose3);
// Assert the graph has 0 error with the correct values.
values.insert(k, pose);
EXPECT_DOUBLES_EQUAL(0, graph.error(values), 1e-16);
// Assert the graph has some error with incorrect values.
values.clear();
Pose3 pose_incorrect{Rot3::RzRyRx(-M_PI, M_PI, -M_PI / 8), Point3(1, 2, 3)};
values.insert(k, pose_incorrect);
EXPECT(0 != graph.error(values));
}
TEST(NonlinearFactorGraph, printErrors)
{
const NonlinearFactorGraph fg = createNonlinearFactorGraph();

16
tests/testNonlinearISAM.cpp
View File

@ -8,7 +8,6 @@
#include <gtsam/slam/BetweenFactor.h>
#include <gtsam/sam/BearingRangeFactor.h>
#include <gtsam/slam/dataset.h>
#include <gtsam/slam/PriorFactor.h>
#include <gtsam/nonlinear/NonlinearEquality.h>
#include <gtsam/nonlinear/NonlinearISAM.h>
#include <gtsam/nonlinear/NonlinearFactorGraph.h>
@ -116,9 +115,9 @@ TEST(testNonlinearISAM, markov_chain_with_disconnects ) {
// Add a floating landmark constellation
if (i == 7) {
new_factors += PriorFactor<Point2>(lm1, landmark1, model2);
new_factors += PriorFactor<Point2>(lm2, landmark2, model2);
new_factors += PriorFactor<Point2>(lm3, landmark3, model2);
new_factors.addPrior(lm1, landmark1, model2);
new_factors.addPrior(lm2, landmark2, model2);
new_factors.addPrior(lm3, landmark3, model2);
// Initialize to origin
new_init.insert(lm1, Point2(0,0));
@ -193,9 +192,9 @@ TEST(testNonlinearISAM, markov_chain_with_reconnect ) {
// Add a floating landmark constellation
if (i == 7) {
new_factors += PriorFactor<Point2>(lm1, landmark1, model2);
new_factors += PriorFactor<Point2>(lm2, landmark2, model2);
new_factors += PriorFactor<Point2>(lm3, landmark3, model2);
new_factors.addPrior(lm1, landmark1, model2);
new_factors.addPrior(lm2, landmark2, model2);
new_factors.addPrior(lm3, landmark3, model2);
// Initialize to origin
new_init.insert(lm1, Point2(0,0));
@ -296,8 +295,7 @@ TEST(testNonlinearISAM, loop_closures ) {
if (id == 0) {
noiseModel::Diagonal::shared_ptr priorNoise =
noiseModel::Diagonal::Sigmas(Vector3(0.001, 0.001, 0.001));
graph.emplace_shared<PriorFactor<Pose2> >(Symbol('x', id),
Pose2(0, 0, 0), priorNoise);
graph.addPrior(Symbol('x', id), Pose2(0, 0, 0), priorNoise);
} else {
isam.update(graph, initialEstimate);

24
tests/testNonlinearOptimizer.cpp
View File

@ -16,7 +16,6 @@
*/
#include <tests/smallExample.h>
#include <gtsam/slam/PriorFactor.h>
#include <gtsam/slam/BetweenFactor.h>
#include <gtsam/nonlinear/NonlinearFactorGraph.h>
#include <gtsam/nonlinear/Values.h>
@ -182,7 +181,7 @@ TEST( NonlinearOptimizer, Factorization )
config.insert(X(2), Pose2(1.5,0.,0.));
NonlinearFactorGraph graph;
graph += PriorFactor<Pose2>(X(1), Pose2(0.,0.,0.), noiseModel::Isotropic::Sigma(3, 1e-10));
graph.addPrior(X(1), Pose2(0.,0.,0.), noiseModel::Isotropic::Sigma(3, 1e-10));
graph += BetweenFactor<Pose2>(X(1),X(2), Pose2(1.,0.,0.), noiseModel::Isotropic::Sigma(3, 1));
Ordering ordering;
@ -241,8 +240,7 @@ TEST(NonlinearOptimizer, NullFactor) {
TEST_UNSAFE(NonlinearOptimizer, MoreOptimization) {
NonlinearFactorGraph fg;
fg += PriorFactor<Pose2>(0, Pose2(0, 0, 0),
noiseModel::Isotropic::Sigma(3, 1));
fg.addPrior(0, Pose2(0, 0, 0), noiseModel::Isotropic::Sigma(3, 1));
fg += BetweenFactor<Pose2>(0, 1, Pose2(1, 0, M_PI / 2),
noiseModel::Isotropic::Sigma(3, 1));
fg += BetweenFactor<Pose2>(1, 2, Pose2(1, 0, M_PI / 2),
@ -343,7 +341,7 @@ TEST_UNSAFE(NonlinearOptimizer, MoreOptimization) {
TEST(NonlinearOptimizer, Pose2OptimizationWithHuberNoOutlier) {
NonlinearFactorGraph fg;
fg += PriorFactor<Pose2>(0, Pose2(0,0,0), noiseModel::Isotropic::Sigma(3,1));
fg.addPrior(0, Pose2(0,0,0), noiseModel::Isotropic::Sigma(3,1));
fg += BetweenFactor<Pose2>(0, 1, Pose2(1,1.1,M_PI/4),
noiseModel::Robust::Create(noiseModel::mEstimator::Huber::Create(2.0),
noiseModel::Isotropic::Sigma(3,1)));
@ -374,7 +372,7 @@ TEST(NonlinearOptimizer, Pose2OptimizationWithHuberNoOutlier) {
TEST(NonlinearOptimizer, Point2LinearOptimizationWithHuber) {
NonlinearFactorGraph fg;
fg += PriorFactor<Point2>(0, Point2(0,0), noiseModel::Isotropic::Sigma(2,0.01));
fg.addPrior(0, Point2(0,0), noiseModel::Isotropic::Sigma(2,0.01));
fg += BetweenFactor<Point2>(0, 1, Point2(1,1.8),
noiseModel::Robust::Create(noiseModel::mEstimator::Huber::Create(1.0),
noiseModel::Isotropic::Sigma(2,1)));
@ -408,7 +406,7 @@ TEST(NonlinearOptimizer, Point2LinearOptimizationWithHuber) {
TEST(NonlinearOptimizer, Pose2OptimizationWithHuber) {
NonlinearFactorGraph fg;
fg += PriorFactor<Pose2>(0, Pose2(0,0, 0), noiseModel::Isotropic::Sigma(3,0.1));
fg.addPrior(0, Pose2(0,0, 0), noiseModel::Isotropic::Sigma(3,0.1));
fg += BetweenFactor<Pose2>(0, 1, Pose2(0,9, M_PI/2),
noiseModel::Robust::Create(noiseModel::mEstimator::Huber::Create(0.2),
noiseModel::Isotropic::Sigma(3,1)));
@ -455,7 +453,7 @@ TEST(NonlinearOptimizer, RobustMeanCalculation) {
vector<double> pts{-10,-3,-1,1,3,10,1000};
for(auto pt : pts) {
fg += PriorFactor<double>(0, pt, huber);
fg.addPrior(0, pt, huber);
}
init.insert(0, 100.0);
@ -485,9 +483,9 @@ TEST(NonlinearOptimizer, disconnected_graph) {
init.insert(X(3), Pose2(0.,0.,0.));
NonlinearFactorGraph graph;
graph += PriorFactor<Pose2>(X(1), Pose2(0.,0.,0.), noiseModel::Isotropic::Sigma(3,1));
graph.addPrior(X(1), Pose2(0.,0.,0.), noiseModel::Isotropic::Sigma(3,1));
graph += BetweenFactor<Pose2>(X(1),X(2), Pose2(1.5,0.,0.), noiseModel::Isotropic::Sigma(3,1));
graph += PriorFactor<Pose2>(X(3), Pose2(3.,0.,0.), noiseModel::Isotropic::Sigma(3,1));
graph.addPrior(X(3), Pose2(3.,0.,0.), noiseModel::Isotropic::Sigma(3,1));
EXPECT(assert_equal(expected, LevenbergMarquardtOptimizer(graph, init).optimize()));
}
@ -530,9 +528,9 @@ TEST(NonlinearOptimizer, subclass_solver) {
init.insert(X(3), Pose2(0.,0.,0.));
NonlinearFactorGraph graph;
graph += PriorFactor<Pose2>(X(1), Pose2(0.,0.,0.), noiseModel::Isotropic::Sigma(3,1));
graph.addPrior(X(1), Pose2(0.,0.,0.), noiseModel::Isotropic::Sigma(3,1));
graph += BetweenFactor<Pose2>(X(1),X(2), Pose2(1.5,0.,0.), noiseModel::Isotropic::Sigma(3,1));
graph += PriorFactor<Pose2>(X(3), Pose2(3.,0.,0.), noiseModel::Isotropic::Sigma(3,1));
graph.addPrior(X(3), Pose2(3.,0.,0.), noiseModel::Isotropic::Sigma(3,1));
ConjugateGradientParameters p;
Values actual = IterativeLM(graph, init, p).optimize();
@ -587,7 +585,7 @@ struct traits<MyType> {
TEST(NonlinearOptimizer, Traits) {
NonlinearFactorGraph fg;
fg += PriorFactor<MyType>(0, MyType(0, 0, 0), noiseModel::Isotropic::Sigma(3, 1));
fg.addPrior(0, MyType(0, 0, 0), noiseModel::Isotropic::Sigma(3, 1));
Values init;
init.insert(0, MyType(0,0,0));

5
tests/testRot3Optimization.cpp
View File

@ -26,11 +26,9 @@
#include <gtsam/nonlinear/GaussNewtonOptimizer.h>
#include <gtsam/inference/Symbol.h>
#include <gtsam/slam/BetweenFactor.h>
#include <gtsam/slam/PriorFactor.h>
using namespace gtsam;
typedef PriorFactor<Rot3> Prior;
typedef BetweenFactor<Rot3> Between;
typedef NonlinearFactorGraph Graph;
@ -41,7 +39,7 @@ TEST(Rot3, optimize) {
Values truth;
Values initial;
Graph fg;
fg += Prior(Symbol('r',0), Rot3(), noiseModel::Isotropic::Sigma(3, 0.01));
fg.addPrior(Symbol('r',0), Rot3(), noiseModel::Isotropic::Sigma(3, 0.01));
for(int j=0; j<6; ++j) {
truth.insert(Symbol('r',j), Rot3::Rz(M_PI/3.0 * double(j)));
initial.insert(Symbol('r',j), Rot3::Rz(M_PI/3.0 * double(j) + 0.1 * double(j%2)));
@ -59,4 +57,3 @@ int main() {
return TestRegistry::runAllTests(tr);
}
/* ************************************************************************* */

2
tests/testSerializationSLAM.cpp
View File

@ -27,7 +27,7 @@
//#include <gtsam/slam/BoundingConstraint.h>
#include <gtsam/slam/GeneralSFMFactor.h>
//#include <gtsam/slam/PartialPriorFactor.h>
#include <gtsam/slam/PriorFactor.h>
#include <gtsam/nonlinear/PriorFactor.h>
#include <gtsam/slam/ProjectionFactor.h>
#include <gtsam/sam/RangeFactor.h>
#include <gtsam/slam/StereoFactor.h>

7
tests/testVisualISAM2.cpp
View File

@ -24,7 +24,6 @@
#include <gtsam/nonlinear/ISAM2.h>
#include <gtsam/nonlinear/NonlinearFactorGraph.h>
#include <gtsam/nonlinear/Values.h>
#include <gtsam/slam/PriorFactor.h>
#include <gtsam/slam/ProjectionFactor.h>
#include <vector>
@ -78,13 +77,11 @@ TEST(testVisualISAM2, all)
static auto kPosePrior = noiseModel::Diagonal::Sigmas(
(Vector(6) << Vector3::Constant(0.1), Vector3::Constant(0.3))
.finished());
graph.emplace_shared<PriorFactor<Pose3>>(Symbol('x', 0), poses[0],
kPosePrior);
graph.addPrior(Symbol('x', 0), poses[0], kPosePrior);
// Add a prior on landmark l0
static auto kPointPrior = noiseModel::Isotropic::Sigma(3, 0.1);
graph.emplace_shared<PriorFactor<Point3>>(Symbol('l', 0), points[0],
kPointPrior);
graph.addPrior(Symbol('l', 0), points[0], kPointPrior);
// Add initial guesses to all observed landmarks
// Intentionally initialize the variables off from the ground truth

3
timing/timeIncremental.cpp
View File

@ -15,7 +15,6 @@
*/
#include <gtsam/slam/dataset.h>
#include <gtsam/slam/PriorFactor.h>
#include <gtsam/slam/BetweenFactor.h>
#include <gtsam/sam/BearingRangeFactor.h>
#include <gtsam/geometry/Pose2.h>
@ -98,7 +97,7 @@ int main(int argc, char *argv[]) {
// cout << "Initializing " << 0 << endl;
newVariables.insert(0, Pose());
// Add prior
newFactors.add(PriorFactor<Pose>(0, Pose(), noiseModel::Unit::Create(3)));
newFactors.addPrior(0, Pose(), noiseModel::Unit::Create(3));
}
while(nextMeasurement < measurements.size()) {

3
timing/timeLago.cpp
View File

@ -17,7 +17,6 @@
*/
#include <gtsam/slam/dataset.h>
#include <gtsam/slam/PriorFactor.h>
#include <gtsam/slam/lago.h>
#include <gtsam/geometry/Pose2.h>
#include <gtsam/nonlinear/GaussNewtonOptimizer.h>
@ -51,7 +50,7 @@ int main(int argc, char *argv[]) {
// Add prior on the pose having index (key) = 0
noiseModel::Diagonal::shared_ptr priorModel = //
noiseModel::Diagonal::Sigmas(Vector3(1e-6, 1e-6, 1e-8));
g->add(PriorFactor<Pose2>(0, Pose2(), priorModel));
g->addPrior(0, Pose2(), priorModel);
// LAGO
for (size_t i = 0; i < trials; i++) {

3
timing/timeiSAM2Chain.cpp
View File

@ -17,7 +17,6 @@
#include <gtsam/base/timing.h>
#include <gtsam/slam/dataset.h>
#include <gtsam/geometry/Pose2.h>
#include <gtsam/slam/PriorFactor.h>
#include <gtsam/slam/BetweenFactor.h>
#include <gtsam/inference/Symbol.h>
#include <gtsam/nonlinear/ISAM2.h>
@ -61,7 +60,7 @@ int main(int argc, char *argv[]) {
gttic_(Create_measurements);
if(step == 0) {
// Add prior
newFactors.add(PriorFactor<Pose>(0, Pose(), noiseModel::Unit::Create(3)));
newFactors.addPrior(0, Pose(), noiseModel::Unit::Create(3));
newVariables.insert(0, Pose());
} else {
Vector eta = Vector::Random(3) * 0.1;