Change all old cases of graph.emplace_shared<PriorFactor<...>>(...) and graph.add(PriorFactor<...>(...)) to graph.addPrior<...>(...). Removed unnecessary PriorFactor.h includes.
alescontrela
parent
f4525b51e4
commit
aa3ac32235
|
|
@ -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/nonlinear/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));
|
||||
|
|
|
|||
|
|
@ -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);
|
||||
|
|
|
|||
|
|
@ -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 =
|
||||
|
|
|
|||
|
|
@ -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
|
||||
|
|
|
|||
|
|
@ -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;
|
||||
|
|
|
|||
|
|
@ -7,7 +7,6 @@
|
|||
#include <gtsam/navigation/Scenario.h>
|
||||
#include <gtsam/nonlinear/ISAM2.h>
|
||||
#include <gtsam/slam/BetweenFactor.h>
|
||||
#include <gtsam/nonlinear/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);
|
||||
|
||||
|
|
|
|||
|
|
@ -41,7 +41,6 @@
|
|||
#include <gtsam/navigation/ImuFactor.h>
|
||||
#include <gtsam/slam/dataset.h>
|
||||
#include <gtsam/slam/BetweenFactor.h>
|
||||
#include <gtsam/nonlinear/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;
|
||||
|
|
|
|||
|
|
@ -22,7 +22,6 @@
|
|||
*/
|
||||
|
||||
#include <gtsam/geometry/Pose2.h>
|
||||
#include <gtsam/nonlinear/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));
|
||||
|
|
|
|||
|
|
@ -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/nonlinear/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);
|
||||
|
|
|
|||
|
|
@ -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/nonlinear/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)
|
||||
|
|
|
|||
|
|
@ -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/nonlinear/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));
|
||||
|
|
|
|||
|
|
@ -19,7 +19,6 @@
|
|||
*/
|
||||
|
||||
#include <gtsam/slam/dataset.h>
|
||||
#include <gtsam/nonlinear/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;
|
||||
|
|
|
|||
|
|
@ -17,7 +17,6 @@
|
|||
*/
|
||||
|
||||
// For an explanation of headers below, please see Pose2SLAMExample.cpp
|
||||
#include <gtsam/nonlinear/PriorFactor.h>
|
||||
#include <gtsam/slam/BetweenFactor.h>
|
||||
#include <gtsam/geometry/Pose2.h>
|
||||
#include <gtsam/nonlinear/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();
|
||||
|
|
|
|||
|
|
@ -17,7 +17,6 @@
|
|||
*/
|
||||
|
||||
// For an explanation of headers below, please see Pose2SLAMExample.cpp
|
||||
#include <gtsam/nonlinear/PriorFactor.h>
|
||||
#include <gtsam/slam/BetweenFactor.h>
|
||||
#include <gtsam/geometry/Pose2.h>
|
||||
#include <gtsam/nonlinear/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));
|
||||
|
|
|
|||
|
|
@ -21,7 +21,6 @@
|
|||
|
||||
#include <gtsam/slam/lago.h>
|
||||
#include <gtsam/slam/dataset.h>
|
||||
#include <gtsam/nonlinear/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;
|
||||
|
|
|
|||
|
|
@ -19,7 +19,6 @@
|
|||
#include <gtsam/nonlinear/NonlinearFactorGraph.h>
|
||||
#include <gtsam/nonlinear/Values.h>
|
||||
#include <gtsam/slam/BetweenFactor.h>
|
||||
#include <gtsam/nonlinear/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);
|
||||
|
|
|
|||
|
|
@ -17,7 +17,6 @@
|
|||
*/
|
||||
|
||||
// For an explanation of headers below, please see Pose2SLAMExample.cpp
|
||||
#include <gtsam/nonlinear/PriorFactor.h>
|
||||
#include <gtsam/slam/BetweenFactor.h>
|
||||
#include <gtsam/geometry/Pose2.h>
|
||||
#include <gtsam/nonlinear/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));
|
||||
|
|
|
|||
|
|
@ -19,7 +19,6 @@
|
|||
|
||||
#include <gtsam/slam/dataset.h>
|
||||
#include <gtsam/slam/BetweenFactor.h>
|
||||
#include <gtsam/nonlinear/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;
|
||||
}
|
||||
|
||||
|
|
|
|||
|
|
@ -19,7 +19,6 @@
|
|||
|
||||
#include <gtsam/slam/dataset.h>
|
||||
#include <gtsam/slam/BetweenFactor.h>
|
||||
#include <gtsam/nonlinear/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;
|
||||
}
|
||||
|
||||
|
|
|
|||
|
|
@ -20,7 +20,6 @@
|
|||
#include <gtsam/slam/InitializePose3.h>
|
||||
#include <gtsam/slam/dataset.h>
|
||||
#include <gtsam/slam/BetweenFactor.h>
|
||||
#include <gtsam/nonlinear/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;
|
||||
}
|
||||
|
||||
|
|
|
|||
|
|
@ -20,7 +20,6 @@
|
|||
#include <gtsam/slam/InitializePose3.h>
|
||||
#include <gtsam/slam/dataset.h>
|
||||
#include <gtsam/slam/BetweenFactor.h>
|
||||
#include <gtsam/nonlinear/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;
|
||||
}
|
||||
|
||||
|
|
|
|||
|
|
@ -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/nonlinear/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);
|
||||
|
||||
|
|
|
|||
|
|
@ -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/nonlinear/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
|
||||
|
|
|
|||
|
|
@ -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");
|
||||
|
||||
|
|
|
|||
|
|
@ -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;
|
||||
|
|
|
|||
|
|
@ -19,7 +19,6 @@
|
|||
#include <gtsam/inference/Symbol.h>
|
||||
#include <gtsam/nonlinear/NonlinearFactorGraph.h>
|
||||
#include <gtsam/nonlinear/LevenbergMarquardtOptimizer.h>
|
||||
#include <gtsam/nonlinear/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;
|
||||
|
|
|
|||
|
|
@ -21,7 +21,6 @@
|
|||
#include <gtsam/inference/Ordering.h>
|
||||
#include <gtsam/nonlinear/NonlinearFactorGraph.h>
|
||||
#include <gtsam/nonlinear/LevenbergMarquardtOptimizer.h>
|
||||
#include <gtsam/nonlinear/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;
|
||||
|
|
|
|||
|
|
@ -33,7 +33,6 @@
|
|||
#include <gtsam/nonlinear/Values.h>
|
||||
|
||||
// SFM-specific factors
|
||||
#include <gtsam/nonlinear/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
|
||||
|
|
|
|||
|
|
@ -34,7 +34,6 @@
|
|||
#include <gtsam/slam/BetweenFactor.h>
|
||||
#include <gtsam/sam/BearingRangeFactor.h>
|
||||
#include <gtsam/slam/dataset.h>
|
||||
#include <gtsam/nonlinear/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;
|
||||
|
|
|
|||
|
|
@ -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/nonlinear/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
|
||||
|
|
|
|||
|
|
@ -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/nonlinear/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);
|
||||
|
|
|
|||
|
|
@ -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();
|
||||
|
|
|
|||
|
|
@ -27,7 +27,6 @@
|
|||
#include <gtsam/nonlinear/ExpressionFactor.h>
|
||||
#include <gtsam/nonlinear/GaussNewtonOptimizer.h>
|
||||
#include <gtsam/nonlinear/NonlinearFactorGraph.h>
|
||||
#include <gtsam/nonlinear/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.
|
||||
|
|
|
|||
|
|
@ -717,7 +717,6 @@ TEST(ImuFactor, bodyPSensorNoBias) {
|
|||
/* ************************************************************************* */
|
||||
#include <gtsam/nonlinear/NonlinearFactorGraph.h>
|
||||
#include <gtsam/slam/BetweenFactor.h>
|
||||
#include <gtsam/nonlinear/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
|
||||
|
|
|
|||
|
|
@ -20,7 +20,6 @@
|
|||
#include <gtsam/nonlinear/GaussNewtonOptimizer.h>
|
||||
#include <gtsam/nonlinear/NonlinearFactorGraph.h>
|
||||
#include <gtsam/slam/KarcherMeanFactor.h>
|
||||
#include <gtsam/nonlinear/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());
|
||||
|
|
|
|||
|
|
@ -17,4 +17,4 @@
|
|||
|
||||
// Note: PriorFactor has been moved to gtsam/nonlinear. This file has been
|
||||
// left here for backwards compatibility.
|
||||
#include <gtsam/nonlinear/PriorFactor.h>
|
||||
#include <gtsam/nonlinear/PriorFactor.h>
|
||||
|
|
|
|||
|
|
@ -18,7 +18,6 @@
|
|||
#include <CppUnitLite/TestHarness.h>
|
||||
|
||||
#include <gtsam/slam/AntiFactor.h>
|
||||
#include <gtsam/nonlinear/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
|
||||
|
|
|
|||
|
|
@ -18,7 +18,6 @@
|
|||
|
||||
#include <gtsam/slam/GeneralSFMFactor.h>
|
||||
#include <gtsam/sam/RangeFactor.h>
|
||||
#include <gtsam/nonlinear/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;
|
||||
|
|
|
|||
|
|
@ -21,7 +21,6 @@
|
|||
#include <gtsam/slam/InitializePose3.h>
|
||||
#include <gtsam/slam/dataset.h>
|
||||
#include <gtsam/slam/BetweenFactor.h>
|
||||
#include <gtsam/nonlinear/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 !!
|
||||
|
|
|
|||
|
|
@ -22,7 +22,6 @@
|
|||
#include <gtsam/slam/lago.h>
|
||||
#include <gtsam/slam/dataset.h>
|
||||
#include <gtsam/slam/BetweenFactor.h>
|
||||
#include <gtsam/nonlinear/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);
|
||||
|
||||
|
|
|
|||
|
|
@ -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);
|
||||
|
|
|
|||
|
|
@ -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);
|
||||
|
||||
|
|
|
|||
|
|
@ -26,7 +26,6 @@
|
|||
#include <gtsam/inference/Symbol.h>
|
||||
#include <gtsam/slam/ProjectionFactor.h>
|
||||
#include <gtsam/slam/GeneralSFMFactor.h>
|
||||
#include <gtsam/nonlinear/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;
|
||||
|
|
|
|||
|
|
@ -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/nonlinear/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;
|
||||
|
||||
|
|
|
|||
|
|
@ -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/nonlinear/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;
|
||||
|
||||
|
|
|
|||
|
|
@ -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/nonlinear/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");
|
||||
|
|
|
|||
|
|
@ -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/nonlinear/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;
|
||||
|
|
|
|||
|
|
@ -17,7 +17,6 @@
|
|||
*/
|
||||
|
||||
#include <gtsam_unstable/geometry/Similarity3.h>
|
||||
#include <gtsam/nonlinear/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);
|
||||
|
|
|
|||
|
|
@ -26,7 +26,6 @@
|
|||
#include <gtsam/linear/GaussianFactorGraph.h>
|
||||
#include <gtsam/nonlinear/NonlinearFactorGraph.h>
|
||||
#include <gtsam/nonlinear/Values.h>
|
||||
#include <gtsam/nonlinear/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;
|
||||
|
||||
|
|
|
|||
|
|
@ -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);
|
||||
|
|
|
|||
|
|
@ -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);
|
||||
|
|
|
|||
|
|
@ -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);
|
||||
|
|
|
|||
|
|
@ -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));
|
||||
|
||||
|
|
|
|||
|
|
@ -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());
|
||||
|
|
|
|||
|
|
@ -18,7 +18,6 @@
|
|||
|
||||
|
||||
#include <gtsam_unstable/nonlinear/IncrementalFixedLagSmoother.h>
|
||||
#include <gtsam/nonlinear/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;
|
||||
|
||||
|
|
|
|||
|
|
@ -6,7 +6,6 @@
|
|||
|
||||
#include <gtsam_unstable/nonlinear/NonlinearClusterTree.h>
|
||||
#include <gtsam/sam/BearingRangeFactor.h>
|
||||
#include <gtsam/nonlinear/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);
|
||||
|
|
|
|||
|
|
@ -13,7 +13,6 @@
|
|||
#include <gtsam/base/numericalDerivative.h>
|
||||
|
||||
#include <gtsam/slam/BetweenFactor.h>
|
||||
#include <gtsam/nonlinear/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);
|
||||
|
||||
|
|
|
|||
|
|
@ -15,7 +15,6 @@
|
|||
#include <gtsam/geometry/Pose2.h>
|
||||
#include <gtsam/geometry/Pose3.h>
|
||||
|
||||
#include <gtsam/nonlinear/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;
|
||||
|
|
|
|||
|
|
@ -19,7 +19,6 @@
|
|||
#include <gtsam_unstable/slam/SmartRangeFactor.h>
|
||||
#include <gtsam/nonlinear/NonlinearFactorGraph.h>
|
||||
#include <gtsam/nonlinear/LevenbergMarquardtOptimizer.h>
|
||||
#include <gtsam/nonlinear/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;
|
||||
|
|
|
|||
|
|
@ -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:
|
||||
|
|
|
|||
|
|
@ -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
|
||||
|
|
|
|||
|
|
@ -401,7 +401,6 @@ TEST(GaussianFactorGraph, hasConstraints)
|
|||
|
||||
#include <gtsam/slam/ProjectionFactor.h>
|
||||
#include <gtsam/geometry/Pose3.h>
|
||||
#include <gtsam/nonlinear/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,
|
||||
|
|
|
|||
|
|
@ -7,7 +7,6 @@
|
|||
#include <gtsam/nonlinear/ISAM2.h>
|
||||
|
||||
#include <tests/smallExample.h>
|
||||
#include <gtsam/nonlinear/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);
|
||||
|
||||
|
|
|
|||
|
|
@ -12,7 +12,6 @@
|
|||
* @date Jun 11, 2012
|
||||
*/
|
||||
|
||||
#include <gtsam/nonlinear/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(
|
||||
|
|
|
|||
|
|
@ -16,7 +16,6 @@
|
|||
**/
|
||||
|
||||
#include <tests/smallExample.h>
|
||||
#include <gtsam/nonlinear/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);
|
||||
|
|
|
|||
|
|
@ -29,7 +29,6 @@
|
|||
#include <gtsam/linear/NoiseModel.h>
|
||||
|
||||
// add in headers for specific factors
|
||||
#include <gtsam/nonlinear/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));
|
||||
|
|
|
|||
|
|
@ -27,7 +27,6 @@
|
|||
#include <gtsam/geometry/Pose2.h>
|
||||
#include <gtsam/geometry/Pose3.h>
|
||||
#include <gtsam/sam/RangeFactor.h>
|
||||
#include <gtsam/nonlinear/PriorFactor.h>
|
||||
#include <gtsam/slam/BetweenFactor.h>
|
||||
|
||||
#include <CppUnitLite/TestHarness.h>
|
||||
|
|
@ -217,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));
|
||||
|
|
@ -273,7 +272,7 @@ TEST(testNonlinearFactorGraph, addPrior) {
|
|||
Eigen::DiagonalMatrix<double, 6, 6> covariance_pose3;
|
||||
covariance_pose3.setIdentity();
|
||||
Pose3 pose{Rot3(), Point3(0, 0, 0)};
|
||||
graph.addPrior<Pose3>(k, pose, covariance_pose3);
|
||||
graph.addPrior<>(k, pose, covariance_pose3);
|
||||
|
||||
// Assert the graph has 0 error with the correct values.
|
||||
values.insert(k, pose);
|
||||
|
|
|
|||
|
|
@ -8,7 +8,6 @@
|
|||
#include <gtsam/slam/BetweenFactor.h>
|
||||
#include <gtsam/sam/BearingRangeFactor.h>
|
||||
#include <gtsam/slam/dataset.h>
|
||||
#include <gtsam/nonlinear/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);
|
||||
|
||||
|
|
|
|||
|
|
@ -16,7 +16,6 @@
|
|||
*/
|
||||
|
||||
#include <tests/smallExample.h>
|
||||
#include <gtsam/nonlinear/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));
|
||||
|
|
|
|||
|
|
@ -26,11 +26,9 @@
|
|||
#include <gtsam/nonlinear/GaussNewtonOptimizer.h>
|
||||
#include <gtsam/inference/Symbol.h>
|
||||
#include <gtsam/slam/BetweenFactor.h>
|
||||
#include <gtsam/nonlinear/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);
|
||||
}
|
||||
/* ************************************************************************* */
|
||||
|
||||
|
|
|
|||
|
|
@ -24,7 +24,6 @@
|
|||
#include <gtsam/nonlinear/ISAM2.h>
|
||||
#include <gtsam/nonlinear/NonlinearFactorGraph.h>
|
||||
#include <gtsam/nonlinear/Values.h>
|
||||
#include <gtsam/nonlinear/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
|
||||
|
|
|
|||
|
|
@ -15,7 +15,6 @@
|
|||
*/
|
||||
|
||||
#include <gtsam/slam/dataset.h>
|
||||
#include <gtsam/nonlinear/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()) {
|
||||
|
||||
|
|
|
|||
|
|
@ -17,7 +17,6 @@
|
|||
*/
|
||||
|
||||
#include <gtsam/slam/dataset.h>
|
||||
#include <gtsam/nonlinear/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++) {
|
||||
|
|
|
|||
|
|
@ -17,7 +17,6 @@
|
|||
#include <gtsam/base/timing.h>
|
||||
#include <gtsam/slam/dataset.h>
|
||||
#include <gtsam/geometry/Pose2.h>
|
||||
#include <gtsam/nonlinear/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;
|
||||
|
|
|
|||
Loading…
Reference in New Issue