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

Replace push_back with emplace_shared. Address PR feedback.

release/4.3a0
alescontrela 2020-04-10 21:32:19 -04:00
parent b266287cca
commit 9887d4467c
2 changed files with 14 additions and 10 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

12
gtsam/nonlinear/NonlinearFactorGraph.h
View File

@ -204,7 +204,7 @@ namespace gtsam {
}
/**
* Directly add a PriorFactor to the factor graph.
* Convenience method which adds a PriorFactor to the factor graph.
* @param key Variable key
* @param prior The variable's prior value
* @param model Noise model for prior factor
@ -212,18 +212,22 @@ namespace gtsam {
template<typename T>
void addPrior(Key key, const T& prior,
const SharedNoiseModel& model = nullptr) {
push_back(boost::make_shared<PriorFactor<T>>(key, prior, model));
emplace_shared<PriorFactor<T>>(key, prior, model);
}
/**
* Directly add a PriorFactor to the factor graph.
* Convenience method which adds a PriorFactor to the factor graph.
* @param key Variable key
* @param prior The variable's prior value
* @param covariance Covariance matrix.
*
* Note that the smart noise model associated with the prior factor
* automatically picks the right noise model (e.g. a diagonal noise model
* if the provided covariance matrix is diagonal).
*/
template<typename T>
void addPrior(Key key, const T& prior, const Matrix& covariance) {
push_back(boost::make_shared<PriorFactor<T>>(key, prior, covariance));
emplace_shared<PriorFactor<T>>(key, prior, covariance);
}
private:

12
tests/testNonlinearFactorGraph.cpp
View File

@ -252,17 +252,17 @@ TEST(testNonlinearFactorGraph, addPrior) {
// Add a prior factor for key k.
auto model_double = noiseModel::Isotropic::Sigma(1, 1);
graph.addPrior<double>(k, 10.0, model_double);
graph.addPrior<double>(k, 10, model_double);
// Assert the graph has 0 error with the correct values.
Values values;
values.insert(k, 10.0);
EXPECT_DOUBLES_EQUAL(0.0, graph.error(values), 1e-16);
EXPECT_DOUBLES_EQUAL(0, graph.error(values), 1e-16);
// Assert the graph has some error with incorrect values.
values.clear();
values.insert(k, 11.0);
EXPECT(0.0 != graph.error(values));
EXPECT(0 != graph.error(values));
// Clear the factor graph and values.
values.clear();
@ -272,18 +272,18 @@ TEST(testNonlinearFactorGraph, addPrior) {
// providing the covariance matrix.
Eigen::DiagonalMatrix<double, 6, 6> covariance_pose3;
covariance_pose3.setIdentity();
Pose3 pose{Rot3(), Point3()};
Pose3 pose{Rot3(), Point3(0, 0, 0)};
graph.addPrior<Pose3>(k, pose, covariance_pose3);
// Assert the graph has 0 error with the correct values.
values.insert(k, pose);
EXPECT_DOUBLES_EQUAL(0.0, graph.error(values), 1e-16);
EXPECT_DOUBLES_EQUAL(0, graph.error(values), 1e-16);
// Assert the graph has some error with incorrect values.
values.clear();
Pose3 pose_incorrect{Rot3::RzRyRx(-M_PI, M_PI, -M_PI / 8), Point3(1, 2, 3)};
values.insert(k, pose_incorrect);
EXPECT(0.0 != graph.error(values));
EXPECT(0 != graph.error(values));
}
/* ************************************************************************* */