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,
const HybridGaussianFactorGraph& newFactors) {
factors += newFactors;
factors.push_back(newFactors);
// Get all the discrete keys from the factors
KeySet allDiscrete = factors.discreteKeySet();

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

@ -483,10 +483,7 @@ TEST(GeneralSFMFactor, BinaryJacobianFactor) {
actualJacobian.augmentedInformation(), 1e-9));
// Construct from GaussianFactorGraph
GaussianFactorGraph gfg1;
gfg1 += expected;
GaussianFactorGraph gfg2;
gfg2 += actual;
GaussianFactorGraph gfg1 {expected}, gfg2 {actual};
HessianFactor hessian1(gfg1), hessian2(gfg2);
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::X;
SymbolicBayesNet bn;
bn += SymbolicConditional(X(3), X(2), A(2));
bn += SymbolicConditional(X(2), X(1), A(1));
bn += SymbolicConditional(X(1));
bn.emplace_shared<SymbolicConditional>(X(3), X(2), A(2));
bn.emplace_shared<SymbolicConditional>(X(2), X(1), A(1));
bn.emplace_shared<SymbolicConditional>(X(1));
DotWriter writer;
writer.positionHints.emplace('a', 2);

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

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

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

@ -110,19 +110,20 @@ TEST(EliminationTree, Create2) {
// \ |
// l3
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 {
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( //
ChildNodes( //

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

@ -292,9 +292,9 @@ TEST(SymbolicFactorGraph, constructFromBayesNet) {
// create Bayes Net
SymbolicBayesNet bayesNet;
bayesNet += SymbolicConditional(0, 1, 2);
bayesNet += SymbolicConditional(1, 2);
bayesNet += SymbolicConditional(1);
bayesNet.emplace_shared<SymbolicConditional>(0, 1, 2);
bayesNet.emplace_shared<SymbolicConditional>(1, 2);
bayesNet.emplace_shared<SymbolicConditional>(1);
// create actual factor graph from a Bayes Net
SymbolicFactorGraph actual(bayesNet);
@ -349,7 +349,7 @@ TEST(SymbolicFactorGraph, push_back) {
TEST(SymbolicFactorGraph, add_factors) {
SymbolicFactorGraph fg1;
fg1.push_factor(10);
fg1 += SymbolicFactor::shared_ptr(); // empty slot!
fg1.push_back(SymbolicFactor::shared_ptr()); // empty slot!
fg1.push_factor(11);
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
SymbolicFactorGraph fullGraph;
fullGraph += asiaGraph;
fullGraph += SymbolicFactor(_B_, _S_);
fullGraph.push_back(asiaGraph);
fullGraph.emplace_shared<SymbolicFactor>(_B_, _S_);
// This ordering is chosen to match the one chosen by COLAMD during the ISAM update
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
PoseRTV x1_v;
gtsam::SharedNoiseModel model1 = gtsam::noiseModel::Unit::Create(1);
gtsam::SharedNoiseModel model3 = gtsam::noiseModel::Unit::Create(3);
gtsam::SharedNoiseModel model6 = gtsam::noiseModel::Unit::Create(6);
gtsam::SharedNoiseModel model9 = gtsam::noiseModel::Unit::Create(9);
SharedNoiseModel model1 = noiseModel::Unit::Create(1);
SharedNoiseModel model3 = noiseModel::Unit::Create(3);
SharedNoiseModel model6 = noiseModel::Unit::Create(6);
SharedNoiseModel model9 = noiseModel::Unit::Create(9);
Vector accel = Vector::Ones(3), gyro = Vector::Ones(3);
IMUFactor<PoseRTV> imu(accel, gyro, 0.01, x1, x2, model6);
FullIMUFactor<PoseRTV> full_imu(accel, gyro, 0.01, x1, x2, model9);
NonlinearEquality<gtsam::PoseRTV> poseHardPrior(x1, x1_v);
BetweenFactor<gtsam::PoseRTV> odom(x1, x2, x1_v, model9);
RangeFactor<gtsam::PoseRTV, gtsam::PoseRTV> range(x1, x2, 1.0, model1);
NonlinearEquality<PoseRTV> poseHardPrior(x1, x1_v);
BetweenFactor<PoseRTV> odom(x1, x2, x1_v, model9);
RangeFactor<PoseRTV, PoseRTV> range(x1, x2, 1.0, model1);
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);
VelocityPrior velPrior(x1, Vector::Ones(3), model3);
}
@ -68,13 +68,13 @@ TEST( testIMUSystem, optimize_chain ) {
// assemble simple graph with IMU measurements and velocity constraints
NonlinearFactorGraph graph;
graph += NonlinearEquality<gtsam::PoseRTV>(x1, pose1);
graph += IMUFactor<PoseRTV>(imu12, dt, x1, x2, model);
graph += IMUFactor<PoseRTV>(imu23, dt, x2, x3, model);
graph += IMUFactor<PoseRTV>(imu34, dt, x3, x4, model);
graph += VelocityConstraint(x1, x2, dt);
graph += VelocityConstraint(x2, x3, dt);
graph += VelocityConstraint(x3, x4, dt);
graph.emplace_shared<NonlinearEquality<PoseRTV>>(x1, pose1);
graph.emplace_shared<IMUFactor<PoseRTV>>(imu12, dt, x1, x2, model);
graph.emplace_shared<IMUFactor<PoseRTV>>(imu23, dt, x2, x3, model);
graph.emplace_shared<IMUFactor<PoseRTV>>(imu34, dt, x3, x4, model);
graph.emplace_shared<VelocityConstraint>(x1, x2, dt);
graph.emplace_shared<VelocityConstraint>(x2, x3, dt);
graph.emplace_shared<VelocityConstraint>(x3, x4, dt);
// ground truth values
Values true_values;
@ -114,10 +114,10 @@ TEST( testIMUSystem, optimize_chain_fullfactor ) {
// assemble simple graph with IMU measurements and velocity constraints
NonlinearFactorGraph graph;
graph += NonlinearEquality<gtsam::PoseRTV>(x1, pose1);
graph += FullIMUFactor<PoseRTV>(imu12, dt, x1, x2, model);
graph += FullIMUFactor<PoseRTV>(imu23, dt, x2, x3, model);
graph += FullIMUFactor<PoseRTV>(imu34, dt, x3, x4, model);
graph.emplace_shared<NonlinearEquality<PoseRTV>>(x1, pose1);
graph.emplace_shared<FullIMUFactor<PoseRTV>>(imu12, dt, x1, x2, model);
graph.emplace_shared<FullIMUFactor<PoseRTV>>(imu23, dt, x2, x3, model);
graph.emplace_shared<FullIMUFactor<PoseRTV>>(imu34, dt, x3, x4, model);
// ground truth values
Values true_values;
@ -156,7 +156,7 @@ TEST( testIMUSystem, linear_trajectory) {
Values true_traj, init_traj;
NonlinearFactorGraph graph;
graph += NonlinearEquality<gtsam::PoseRTV>(x0, start);
graph.emplace_shared<NonlinearEquality<PoseRTV>>(x0, start);
true_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) {
Key xA = i-1, xB = i;
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);
init_traj.insert(xB, PoseRTV());
}

2
gtsam_unstable/nonlinear/ConcurrentFilteringAndSmoothing.cpp
View File

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

5
gtsam_unstable/slam/SmartStereoProjectionFactor.h
View File

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

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

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

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

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