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

More emplace_shared usage

release/4.3a0
Frank Dellaert 2023-02-06 01:12:51 -08:00
parent 29c25970ad
commit a9971fd2fd
12 changed files with 56 additions and 58 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
gtsam/hybrid/tests/testHybridEstimation.cpp
View File

@ -44,7 +44,7 @@ using symbol_shorthand::X;
Ordering getOrdering(HybridGaussianFactorGraph& factors, Ordering getOrdering(HybridGaussianFactorGraph& factors,
const HybridGaussianFactorGraph& newFactors) { const HybridGaussianFactorGraph& newFactors) {
factors += newFactors; factors.push_back(newFactors);
// Get all the discrete keys from the factors // Get all the discrete keys from the factors
KeySet allDiscrete = factors.discreteKeySet(); KeySet allDiscrete = factors.discreteKeySet();

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

@ -483,10 +483,7 @@ TEST(GeneralSFMFactor, BinaryJacobianFactor) {
actualJacobian.augmentedInformation(), 1e-9)); actualJacobian.augmentedInformation(), 1e-9));
// Construct from GaussianFactorGraph // Construct from GaussianFactorGraph
GaussianFactorGraph gfg1; GaussianFactorGraph gfg1 {expected}, gfg2 {actual};
gfg1 += expected;
GaussianFactorGraph gfg2;
gfg2 += actual;
HessianFactor hessian1(gfg1), hessian2(gfg2); HessianFactor hessian1(gfg1), hessian2(gfg2);
EXPECT(assert_equal(hessian1, hessian2, 1e-9)); EXPECT(assert_equal(hessian1, hessian2, 1e-9));
} }

6
gtsam/symbolic/tests/testSymbolicBayesNet.cpp
View File

@ -83,9 +83,9 @@ TEST(SymbolicBayesNet, Dot) {
using symbol_shorthand::A; using symbol_shorthand::A;
using symbol_shorthand::X; using symbol_shorthand::X;
SymbolicBayesNet bn; SymbolicBayesNet bn;
bn += SymbolicConditional(X(3), X(2), A(2)); bn.emplace_shared<SymbolicConditional>(X(3), X(2), A(2));
bn += SymbolicConditional(X(2), X(1), A(1)); bn.emplace_shared<SymbolicConditional>(X(2), X(1), A(1));
bn += SymbolicConditional(X(1)); bn.emplace_shared<SymbolicConditional>(X(1));
DotWriter writer; DotWriter writer;
writer.positionHints.emplace('a', 2); writer.positionHints.emplace('a', 2);

4
gtsam/symbolic/tests/testSymbolicBayesTree.cpp
View File

@ -257,8 +257,8 @@ TEST(BayesTree, removeTop) {
// Check expected outcome // Check expected outcome
SymbolicBayesNet expected; SymbolicBayesNet expected;
expected += SymbolicConditional::FromKeys<KeyVector>(Keys(_E_)(_L_)(_B_), 3); expected.add(SymbolicConditional::FromKeys<KeyVector>(Keys(_E_)(_L_)(_B_), 3));
expected += SymbolicConditional::FromKeys<KeyVector>(Keys(_S_)(_B_)(_L_), 1); expected.add(SymbolicConditional::FromKeys<KeyVector>(Keys(_S_)(_B_)(_L_), 1));
CHECK(assert_equal(expected, bn)); CHECK(assert_equal(expected, bn));
SymbolicBayesTree::Cliques expectedOrphans{bayesTree[_T_], bayesTree[_X_]}; SymbolicBayesTree::Cliques expectedOrphans{bayesTree[_T_], bayesTree[_X_]};

21
gtsam/symbolic/tests/testSymbolicEliminationTree.cpp
View File

@ -110,19 +110,20 @@ TEST(EliminationTree, Create2) {
// \ | // \ |
// l3 // l3
SymbolicFactorGraph graph; SymbolicFactorGraph graph;
graph.emplace_shared<SymbolicFactor>(X(1), L(1));
graph.emplace_shared<SymbolicFactor>(X(1), X(2));
graph.emplace_shared<SymbolicFactor>(X(2), L(1));
graph.emplace_shared<SymbolicFactor>(X(2), X(3));
graph.emplace_shared<SymbolicFactor>(X(3), X(4));
graph.emplace_shared<SymbolicFactor>(X(4), L(2));
graph.emplace_shared<SymbolicFactor>(X(4), X(5));
graph.emplace_shared<SymbolicFactor>(L(2), X(5));
graph.emplace_shared<SymbolicFactor>(X(4), L(3));
graph.emplace_shared<SymbolicFactor>(X(5), L(3));
auto binary = [](Key j1, Key j2) -> SymbolicFactor { auto binary = [](Key j1, Key j2) -> SymbolicFactor {
return SymbolicFactor(j1, j2); return SymbolicFactor(j1, j2);
}; };
graph += binary(X(1), L(1));
graph += binary(X(1), X(2));
graph += binary(X(2), L(1));
graph += binary(X(2), X(3));
graph += binary(X(3), X(4));
graph += binary(X(4), L(2));
graph += binary(X(4), X(5));
graph += binary(L(2), X(5));
graph += binary(X(4), L(3));
graph += binary(X(5), L(3));
SymbolicEliminationTree expected = EliminationTreeTester::MakeTree( // SymbolicEliminationTree expected = EliminationTreeTester::MakeTree( //
ChildNodes( // ChildNodes( //

8
gtsam/symbolic/tests/testSymbolicFactorGraph.cpp
View File

@ -292,9 +292,9 @@ TEST(SymbolicFactorGraph, constructFromBayesNet) {
// create Bayes Net // create Bayes Net
SymbolicBayesNet bayesNet; SymbolicBayesNet bayesNet;
bayesNet += SymbolicConditional(0, 1, 2); bayesNet.emplace_shared<SymbolicConditional>(0, 1, 2);
bayesNet += SymbolicConditional(1, 2); bayesNet.emplace_shared<SymbolicConditional>(1, 2);
bayesNet += SymbolicConditional(1); bayesNet.emplace_shared<SymbolicConditional>(1);
// create actual factor graph from a Bayes Net // create actual factor graph from a Bayes Net
SymbolicFactorGraph actual(bayesNet); SymbolicFactorGraph actual(bayesNet);
@ -349,7 +349,7 @@ TEST(SymbolicFactorGraph, push_back) {
TEST(SymbolicFactorGraph, add_factors) { TEST(SymbolicFactorGraph, add_factors) {
SymbolicFactorGraph fg1; SymbolicFactorGraph fg1;
fg1.push_factor(10); fg1.push_factor(10);
fg1 += SymbolicFactor::shared_ptr(); // empty slot! fg1.push_back(SymbolicFactor::shared_ptr()); // empty slot!
fg1.push_factor(11); fg1.push_factor(11);
SymbolicFactorGraph fg2; SymbolicFactorGraph fg2;

4
gtsam/symbolic/tests/testSymbolicISAM.cpp
View File

@ -79,8 +79,8 @@ TEST( SymbolicISAM, iSAM )
// Now we modify the Bayes tree by inserting a new factor over B and S // Now we modify the Bayes tree by inserting a new factor over B and S
SymbolicFactorGraph fullGraph; SymbolicFactorGraph fullGraph;
fullGraph += asiaGraph; fullGraph.push_back(asiaGraph);
fullGraph += SymbolicFactor(_B_, _S_); fullGraph.emplace_shared<SymbolicFactor>(_B_, _S_);
// This ordering is chosen to match the one chosen by COLAMD during the ISAM update // This ordering is chosen to match the one chosen by COLAMD during the ISAM update
Ordering ordering {_X_, _B_, _S_, _E_, _L_, _T_}; Ordering ordering {_X_, _B_, _S_, _E_, _L_, _T_};

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

@ -33,20 +33,20 @@ TEST(testIMUSystem, instantiations) {
// just checking for compilation // just checking for compilation
PoseRTV x1_v; PoseRTV x1_v;
gtsam::SharedNoiseModel model1 = gtsam::noiseModel::Unit::Create(1); SharedNoiseModel model1 = noiseModel::Unit::Create(1);
gtsam::SharedNoiseModel model3 = gtsam::noiseModel::Unit::Create(3); SharedNoiseModel model3 = noiseModel::Unit::Create(3);
gtsam::SharedNoiseModel model6 = gtsam::noiseModel::Unit::Create(6); SharedNoiseModel model6 = noiseModel::Unit::Create(6);
gtsam::SharedNoiseModel model9 = gtsam::noiseModel::Unit::Create(9); SharedNoiseModel model9 = noiseModel::Unit::Create(9);
Vector accel = Vector::Ones(3), gyro = Vector::Ones(3); Vector accel = Vector::Ones(3), gyro = Vector::Ones(3);
IMUFactor<PoseRTV> imu(accel, gyro, 0.01, x1, x2, model6); IMUFactor<PoseRTV> imu(accel, gyro, 0.01, x1, x2, model6);
FullIMUFactor<PoseRTV> full_imu(accel, gyro, 0.01, x1, x2, model9); FullIMUFactor<PoseRTV> full_imu(accel, gyro, 0.01, x1, x2, model9);
NonlinearEquality<gtsam::PoseRTV> poseHardPrior(x1, x1_v); NonlinearEquality<PoseRTV> poseHardPrior(x1, x1_v);
BetweenFactor<gtsam::PoseRTV> odom(x1, x2, x1_v, model9); BetweenFactor<PoseRTV> odom(x1, x2, x1_v, model9);
RangeFactor<gtsam::PoseRTV, gtsam::PoseRTV> range(x1, x2, 1.0, model1); RangeFactor<PoseRTV, PoseRTV> range(x1, x2, 1.0, model1);
VelocityConstraint constraint(x1, x2, 0.1, 10000); VelocityConstraint constraint(x1, x2, 0.1, 10000);
PriorFactor<gtsam::PoseRTV> posePrior(x1, x1_v, model9); PriorFactor<PoseRTV> posePrior(x1, x1_v, model9);
DHeightPrior heightPrior(x1, 0.1, model1); DHeightPrior heightPrior(x1, 0.1, model1);
VelocityPrior velPrior(x1, Vector::Ones(3), model3); VelocityPrior velPrior(x1, Vector::Ones(3), model3);
} }
@ -68,13 +68,13 @@ TEST( testIMUSystem, optimize_chain ) {
// assemble simple graph with IMU measurements and velocity constraints // assemble simple graph with IMU measurements and velocity constraints
NonlinearFactorGraph graph; NonlinearFactorGraph graph;
graph += NonlinearEquality<gtsam::PoseRTV>(x1, pose1); graph.emplace_shared<NonlinearEquality<PoseRTV>>(x1, pose1);
graph += IMUFactor<PoseRTV>(imu12, dt, x1, x2, model); graph.emplace_shared<IMUFactor<PoseRTV>>(imu12, dt, x1, x2, model);
graph += IMUFactor<PoseRTV>(imu23, dt, x2, x3, model); graph.emplace_shared<IMUFactor<PoseRTV>>(imu23, dt, x2, x3, model);
graph += IMUFactor<PoseRTV>(imu34, dt, x3, x4, model); graph.emplace_shared<IMUFactor<PoseRTV>>(imu34, dt, x3, x4, model);
graph += VelocityConstraint(x1, x2, dt); graph.emplace_shared<VelocityConstraint>(x1, x2, dt);
graph += VelocityConstraint(x2, x3, dt); graph.emplace_shared<VelocityConstraint>(x2, x3, dt);
graph += VelocityConstraint(x3, x4, dt); graph.emplace_shared<VelocityConstraint>(x3, x4, dt);
// ground truth values // ground truth values
Values true_values; Values true_values;
@ -114,10 +114,10 @@ TEST( testIMUSystem, optimize_chain_fullfactor ) {
// assemble simple graph with IMU measurements and velocity constraints // assemble simple graph with IMU measurements and velocity constraints
NonlinearFactorGraph graph; NonlinearFactorGraph graph;
graph += NonlinearEquality<gtsam::PoseRTV>(x1, pose1); graph.emplace_shared<NonlinearEquality<PoseRTV>>(x1, pose1);
graph += FullIMUFactor<PoseRTV>(imu12, dt, x1, x2, model); graph.emplace_shared<FullIMUFactor<PoseRTV>>(imu12, dt, x1, x2, model);
graph += FullIMUFactor<PoseRTV>(imu23, dt, x2, x3, model); graph.emplace_shared<FullIMUFactor<PoseRTV>>(imu23, dt, x2, x3, model);
graph += FullIMUFactor<PoseRTV>(imu34, dt, x3, x4, model); graph.emplace_shared<FullIMUFactor<PoseRTV>>(imu34, dt, x3, x4, model);
// ground truth values // ground truth values
Values true_values; Values true_values;
@ -156,7 +156,7 @@ TEST( testIMUSystem, linear_trajectory) {
Values true_traj, init_traj; Values true_traj, init_traj;
NonlinearFactorGraph graph; NonlinearFactorGraph graph;
graph += NonlinearEquality<gtsam::PoseRTV>(x0, start); graph.emplace_shared<NonlinearEquality<PoseRTV>>(x0, start);
true_traj.insert(x0, start); true_traj.insert(x0, start);
init_traj.insert(x0, start); init_traj.insert(x0, start);
@ -165,7 +165,7 @@ TEST( testIMUSystem, linear_trajectory) {
for (size_t i=1; i<nrPoses; ++i) { for (size_t i=1; i<nrPoses; ++i) {
Key xA = i-1, xB = i; Key xA = i-1, xB = i;
cur_pose = cur_pose.generalDynamics(accel, gyro, dt); cur_pose = cur_pose.generalDynamics(accel, gyro, dt);
graph += FullIMUFactor<PoseRTV>(accel - g, gyro, dt, xA, xB, model); graph.emplace_shared<FullIMUFactor<PoseRTV>>(accel - g, gyro, dt, xA, xB, model);
true_traj.insert(xB, cur_pose); true_traj.insert(xB, cur_pose);
init_traj.insert(xB, PoseRTV()); init_traj.insert(xB, PoseRTV());
} }

2
gtsam_unstable/nonlinear/ConcurrentFilteringAndSmoothing.cpp
View File

@ -78,7 +78,7 @@ NonlinearFactorGraph calculateMarginalFactors(const NonlinearFactorGraph& graph,
NonlinearFactorGraph marginalFactors; NonlinearFactorGraph marginalFactors;
marginalFactors.reserve(marginalLinearFactors.size()); marginalFactors.reserve(marginalLinearFactors.size());
for(const GaussianFactor::shared_ptr& gaussianFactor: marginalLinearFactors) { for(const GaussianFactor::shared_ptr& gaussianFactor: marginalLinearFactors) {
marginalFactors += std::make_shared<LinearContainerFactor>(gaussianFactor, theta); marginalFactors.emplace_shared<LinearContainerFactor>(gaussianFactor, theta);
} }
return marginalFactors; return marginalFactors;

5
gtsam_unstable/slam/SmartStereoProjectionFactor.h
View File

@ -30,7 +30,10 @@
#include <gtsam/slam/dataset.h> #include <gtsam/slam/dataset.h>
#include <gtsam_unstable/dllexport.h> #include <gtsam_unstable/dllexport.h>
#ifdef GTSAM_ENABLE_BOOST_SERIALIZATION
#include <boost/serialization/optional.hpp> #include <boost/serialization/optional.hpp>
#endif
#include <optional> #include <optional>
#include <vector> #include <vector>
@ -501,7 +504,7 @@ public:
private: private:
#ifdef GTSAM_ENABLE_BOOST_SERIALIZATION /// #ifdef GTSAM_ENABLE_BOOST_SERIALIZATION
/// Serialization function /// Serialization function
friend class boost::serialization::access; friend class boost::serialization::access;
template<class ARCHIVE> template<class ARCHIVE>

9
gtsam_unstable/slam/tests/testInvDepthFactor3.cpp
View File

@ -62,10 +62,9 @@ TEST( InvDepthFactor, optimize) {
gtsam::NonlinearFactorGraph graph; gtsam::NonlinearFactorGraph graph;
Values initial; Values initial;
InverseDepthFactor::shared_ptr factor(new InverseDepthFactor(expected_uv, sigma, graph.emplace_shared<InverseDepthFactor>(expected_uv, sigma,
Symbol('x',1), Symbol('l',1), Symbol('d',1), K)); Symbol('x',1), Symbol('l',1), Symbol('d',1), K);
graph.push_back(factor); graph.emplace_shared<PoseConstraint>(Symbol('x', 1), level_pose);
graph += PoseConstraint(Symbol('x',1),level_pose);
initial.insert(Symbol('x',1), level_pose); initial.insert(Symbol('x',1), level_pose);
initial.insert(Symbol('l',1), inv_landmark); initial.insert(Symbol('l',1), inv_landmark);
initial.insert(Symbol('d',1), inv_depth); initial.insert(Symbol('d',1), inv_depth);
@ -91,7 +90,7 @@ TEST( InvDepthFactor, optimize) {
Symbol('x',2), Symbol('l',1),Symbol('d',1),K)); Symbol('x',2), Symbol('l',1),Symbol('d',1),K));
graph.push_back(factor1); graph.push_back(factor1);
graph += PoseConstraint(Symbol('x',2),right_pose); graph.emplace_shared<PoseConstraint>(Symbol('x',2),right_pose);
initial.insert(Symbol('x',2), right_pose); initial.insert(Symbol('x',2), right_pose);

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

@ -73,10 +73,8 @@ TEST( MultiProjectionFactor, create ){
views.insert(x2); views.insert(x2);
views.insert(x3); views.insert(x3);
MultiProjectionFactor<Pose3, Point3> mpFactor(n_measPixel, noiseProjection, views, l1, K); graph.emplace_shared<MultiProjectionFactor<Pose3, Point3>>(
graph += mpFactor; n_measPixel, noiseProjection, views, l1, K);
} }