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

Replaced graph.push_back with graph.emplace_shared if needed.

release/4.3a0
Yao Chen 2016-10-01 11:17:41 -04:00
parent 95c75b8bae
commit 249d6b0b1b
25 changed files with 215 additions and 232 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

7
examples/SFMExample_bal_COLAMD_METIS.cpp
View File

@ -64,15 +64,15 @@ int main (int argc, char* argv[]) {
for(const SfM_Measurement& m: track.measurements) {
size_t i = m.first;
Point2 uv = m.second;
graph.push_back(MyFactor(uv, noise, C(i), P(j))); // note use of shorthand symbols C and P
graph.emplace_shared<MyFactor>(uv, noise, C(i), P(j)); // note use of shorthand symbols C and P
}
j += 1;
}
// 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.push_back(PriorFactor<SfM_Camera>(C(0), mydata.cameras[0], noiseModel::Isotropic::Sigma(9, 0.1)));
graph.push_back(PriorFactor<Point3> (P(0), mydata.tracks[0].p, noiseModel::Isotropic::Sigma(3, 0.1)));
graph.emplace_shared<PriorFactor<SfM_Camera> >(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));
// Create initial estimate
Values initial;
@ -82,7 +82,6 @@ int main (int argc, char* argv[]) {
/** --------------- COMPARISON -----------------------**/
/** ----------------------------------------------------**/
M\]
LevenbergMarquardtParams params_using_COLAMD, params_using_METIS;
try {

8
examples/SelfCalibrationExample.cpp
View File

@ -54,7 +54,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.3), Vector3::Constant(0.1)).finished()); // 30cm std on x,y,z 0.1 rad on roll,pitch,yaw
graph.push_back(PriorFactor<Pose3>(Symbol('x', 0), poses[0], poseNoise));
graph.emplace_shared<PriorFactor<Pose3> >(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);
@ -65,17 +65,17 @@ int main(int argc, char* argv[]) {
Point2 measurement = camera.project(points[j]);
// The only real difference with the Visual SLAM example is that here we use a
// different factor type, that also calculates the Jacobian with respect to calibration
graph.push_back(GeneralSFMFactor2<Cal3_S2>(measurement, measurementNoise, Symbol('x', i), Symbol('l', j), Symbol('K', 0)));
graph.emplace_shared<GeneralSFMFactor2<Cal3_S2> >(measurement, measurementNoise, Symbol('x', i), Symbol('l', j), Symbol('K', 0));
}
}
// Add a prior on the position of the first landmark.
noiseModel::Isotropic::shared_ptr pointNoise = noiseModel::Isotropic::Sigma(3, 0.1);
graph.push_back(PriorFactor<Point3>(Symbol('l', 0), points[0], pointNoise)); // add directly to graph
graph.emplace_shared<PriorFactor<Point3> >(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.push_back(PriorFactor<Cal3_S2>(Symbol('K', 0), K, calNoise));
graph.emplace_shared<PriorFactor<Cal3_S2> >(Symbol('K', 0), K, calNoise);
// Create the initial estimate to the solution
// now including an estimate on the camera calibration parameters

14
examples/StereoVOExample.cpp
View File

@ -39,7 +39,7 @@ int main(int argc, char** argv){
//create graph object, add first pose at origin with key '1'
NonlinearFactorGraph graph;
Pose3 first_pose;
graph.push_back(NonlinearEquality<Pose3>(1, Pose3()));
graph.emplace_shared<NonlinearEquality<Pose3> >(1, Pose3());
//create factor noise model with 3 sigmas of value 1
const noiseModel::Isotropic::shared_ptr model = noiseModel::Isotropic::Sigma(3,1);
@ -47,14 +47,14 @@ int main(int argc, char** argv){
const Cal3_S2Stereo::shared_ptr K(new Cal3_S2Stereo(1000, 1000, 0, 320, 240, 0.2));
//create and add stereo factors between first pose (key value 1) and the three landmarks
graph.push_back(GenericStereoFactor<Pose3,Point3>(StereoPoint2(520, 480, 440), model, 1, 3, K));
graph.push_back(GenericStereoFactor<Pose3,Point3>(StereoPoint2(120, 80, 440), model, 1, 4, K));
graph.push_back(GenericStereoFactor<Pose3,Point3>(StereoPoint2(320, 280, 140), model, 1, 5, K));
graph.emplace_shared<GenericStereoFactor<Pose3,Point3> >(StereoPoint2(520, 480, 440), model, 1, 3, K);
graph.emplace_shared<GenericStereoFactor<Pose3,Point3> >(StereoPoint2(120, 80, 440), model, 1, 4, K);
graph.emplace_shared<GenericStereoFactor<Pose3,Point3> >(StereoPoint2(320, 280, 140), model, 1, 5, K);
//create and add stereo factors between second pose and the three landmarks
graph.push_back(GenericStereoFactor<Pose3,Point3>(StereoPoint2(570, 520, 490), model, 2, 3, K));
graph.push_back(GenericStereoFactor<Pose3,Point3>(StereoPoint2(70, 20, 490), model, 2, 4, K));
graph.push_back(GenericStereoFactor<Pose3,Point3>(StereoPoint2(320, 270, 115), model, 2, 5, K));
graph.emplace_shared<GenericStereoFactor<Pose3,Point3> >(StereoPoint2(570, 520, 490), model, 2, 3, K);
graph.emplace_shared<GenericStereoFactor<Pose3,Point3> >(StereoPoint2(70, 20, 490), model, 2, 4, K);
graph.emplace_shared<GenericStereoFactor<Pose3,Point3> >(StereoPoint2(320, 270, 115), model, 2, 5, K);
//create Values object to contain initial estimates of camera poses and landmark locations
Values initial_estimate;

8
examples/StereoVOExample_large.cpp
View File

@ -83,9 +83,9 @@ int main(int argc, char** argv){
cout << "Reading stereo factors" << endl;
//read stereo measurement details from file and use to create and add GenericStereoFactor objects to the graph representation
while (factor_file >> x >> l >> uL >> uR >> v >> X >> Y >> Z) {
graph.push_back(
GenericStereoFactor<Pose3, Point3>(StereoPoint2(uL, uR, v), model,
Symbol('x', x), Symbol('l', l), K));
graph.emplace_shared<
GenericStereoFactor<Pose3, Point3> >(StereoPoint2(uL, uR, v), model,
Symbol('x', x), Symbol('l', l), K);
//if the landmark variable included in this factor has not yet been added to the initial variable value estimate, add it
if (!initial_estimate.exists(Symbol('l', l))) {
Pose3 camPose = initial_estimate.at<Pose3>(Symbol('x', x));
@ -99,7 +99,7 @@ int main(int argc, char** argv){
//constrain the first pose such that it cannot change from its original value during optimization
// NOTE: NonlinearEquality forces the optimizer to use QR rather than Cholesky
// QR is much slower than Cholesky, but numerically more stable
graph.push_back(NonlinearEquality<Pose3>(Symbol('x',1),first_pose));
graph.emplace_shared<NonlinearEquality<Pose3> >(Symbol('x',1),first_pose);
cout << "Optimizing" << endl;
//create Levenberg-Marquardt optimizer to optimize the factor graph

6
examples/VisualISAM2Example.cpp
View File

@ -90,7 +90,7 @@ int main(int argc, char* argv[]) {
for (size_t j = 0; j < points.size(); ++j) {
SimpleCamera camera(poses[i], *K);
Point2 measurement = camera.project(points[j]);
graph.push_back(GenericProjectionFactor<Pose3, Point3, Cal3_S2>(measurement, measurementNoise, Symbol('x', i), Symbol('l', j), K));
graph.emplace_shared<GenericProjectionFactor<Pose3, Point3, Cal3_S2> >(measurement, measurementNoise, Symbol('x', i), Symbol('l', j), K);
}
// Add an initial guess for the current pose
@ -104,11 +104,11 @@ int main(int argc, char* argv[]) {
if( i == 0) {
// Add a prior on pose x0
noiseModel::Diagonal::shared_ptr poseNoise = noiseModel::Diagonal::Sigmas((Vector(6) << Vector3::Constant(0.3),Vector3::Constant(0.1)).finished()); // 30cm std on x,y,z 0.1 rad on roll,pitch,yaw
graph.push_back(PriorFactor<Pose3>(Symbol('x', 0), poses[0], poseNoise));
graph.emplace_shared<PriorFactor<Pose3> >(Symbol('x', 0), poses[0], poseNoise);
// Add a prior on landmark l0
noiseModel::Isotropic::shared_ptr pointNoise = noiseModel::Isotropic::Sigma(3, 0.1);
graph.push_back(PriorFactor<Point3>(Symbol('l', 0), points[0], pointNoise)); // add directly to graph
graph.emplace_shared<PriorFactor<Point3> >(Symbol('l', 0), points[0], pointNoise); // add directly to graph
// Add initial guesses to all observed landmarks
// Intentionally initialize the variables off from the ground truth

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

@ -378,12 +378,12 @@ TEST( triangulation, StereotriangulateNonlinear ) {
NonlinearFactorGraph graph;
static SharedNoiseModel unit(noiseModel::Unit::Create(3));
graph.push_back(StereoFactor::shared_ptr(new StereoFactor(measurements[0], unit, Symbol('x',1), Symbol('l',1), stereoK)));
graph.push_back(StereoFactor::shared_ptr(new StereoFactor(measurements[1], unit, Symbol('x',2), Symbol('l',1), stereoK)));
graph.emplace_shared<StereoFactor>(measurements[0], unit, Symbol('x',1), Symbol('l',1), stereoK);
graph.emplace_shared<StereoFactor>(measurements[1], unit, Symbol('x',2), Symbol('l',1), stereoK);
const SharedDiagonal posePrior = noiseModel::Isotropic::Sigma(6, 1e-9);
graph.push_back(PriorFactor<Pose3>(Symbol('x',1), Pose3(m1), posePrior));
graph.push_back(PriorFactor<Pose3>(Symbol('x',2), Pose3(m2), posePrior));
graph.emplace_shared<PriorFactor<Pose3> >(Symbol('x',1), Pose3(m1), posePrior);
graph.emplace_shared<PriorFactor<Pose3> >(Symbol('x',2), Pose3(m2), posePrior);
LevenbergMarquardtOptimizer optimizer(graph, values);
Values result = optimizer.optimize();
@ -399,8 +399,8 @@ TEST( triangulation, StereotriangulateNonlinear ) {
NonlinearFactorGraph graph;
static SharedNoiseModel unit(noiseModel::Unit::Create(3));
graph.push_back(TriangulationFactor<StereoCamera>(cameras[0], measurements[0], unit, Symbol('l',1)));
graph.push_back(TriangulationFactor<StereoCamera>(cameras[1], measurements[1], unit, Symbol('l',1)));
graph.emplace_shared<TriangulationFactor<StereoCamera> >(cameras[0], measurements[0], unit, Symbol('l',1));
graph.emplace_shared<TriangulationFactor<StereoCamera> >(cameras[1], measurements[1], unit, Symbol('l',1));
LevenbergMarquardtOptimizer optimizer(graph, values);
Values result = optimizer.optimize();

8
gtsam/geometry/triangulation.h
View File

@ -89,8 +89,8 @@ std::pair<NonlinearFactorGraph, Values> triangulationGraph(
const Pose3& pose_i = poses[i];
typedef PinholePose<CALIBRATION> Camera;
Camera camera_i(pose_i, sharedCal);
graph.push_back(TriangulationFactor<Camera> //
(camera_i, measurements[i], unit2, landmarkKey));
graph.emplace_shared<TriangulationFactor<Camera> > //
(camera_i, measurements[i], unit2, landmarkKey);
}
return std::make_pair(graph, values);
}
@ -116,8 +116,8 @@ std::pair<NonlinearFactorGraph, Values> triangulationGraph(
traits<typename CAMERA::Measurement>::dimension));
for (size_t i = 0; i < measurements.size(); i++) {
const CAMERA& camera_i = cameras[i];
graph.push_back(TriangulationFactor<CAMERA> //
(camera_i, measurements[i], unit, landmarkKey));
graph.emplace_shared<TriangulationFactor<CAMERA> > //
(camera_i, measurements[i], unit, landmarkKey);
}
return std::make_pair(graph, values);
}

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

@ -90,8 +90,8 @@ TEST( AntiFactor, EquivalentBayesNet)
SharedNoiseModel sigma(noiseModel::Unit::Create(6));
NonlinearFactorGraph graph;
graph.push_back(PriorFactor<Pose3>(1, pose1, sigma));
graph.push_back(BetweenFactor<Pose3>(1, 2, pose1.between(pose2), sigma));
graph.emplace_shared<PriorFactor<Pose3> >(1, pose1, sigma);
graph.emplace_shared<BetweenFactor<Pose3> >(1, 2, pose1.between(pose2), sigma);
// Create a configuration corresponding to the ground truth
Values values;

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

@ -368,9 +368,9 @@ TEST( GeneralSFMFactor, optimize_varK_BA ) {
graph.addCameraConstraint(0, cameras[0]);
// Constrain the scale of the problem with a soft range factor of 1m between the cameras
graph.push_back(
RangeFactor<GeneralCamera, GeneralCamera>(X(0), X(1), 2.,
noiseModel::Isotropic::Sigma(1, 10.)));
graph.emplace_shared<
RangeFactor<GeneralCamera, GeneralCamera> >(X(0), X(1), 2.,
noiseModel::Isotropic::Sigma(1, 10.));
const double reproj_error = 1e-5;
@ -385,12 +385,12 @@ TEST(GeneralSFMFactor, GeneralCameraPoseRange) {
// Tests range factor between a GeneralCamera and a Pose3
Graph graph;
graph.addCameraConstraint(0, GeneralCamera());
graph.push_back(
RangeFactor<GeneralCamera, Pose3>(X(0), X(1), 2.,
noiseModel::Isotropic::Sigma(1, 1.)));
graph.push_back(
PriorFactor<Pose3>(X(1), Pose3(Rot3(), Point3(1., 0., 0.)),
noiseModel::Isotropic::Sigma(6, 1.)));
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.)),
noiseModel::Isotropic::Sigma(6, 1.));
Values init;
init.insert(X(0), GeneralCamera());
@ -413,15 +413,15 @@ TEST(GeneralSFMFactor, GeneralCameraPoseRange) {
TEST(GeneralSFMFactor, CalibratedCameraPoseRange) {
// Tests range factor between a CalibratedCamera and a Pose3
NonlinearFactorGraph graph;
graph.push_back(
PriorFactor<CalibratedCamera>(X(0), CalibratedCamera(),
noiseModel::Isotropic::Sigma(6, 1.)));
graph.push_back(
RangeFactor<CalibratedCamera, Pose3>(X(0), X(1), 2.,
noiseModel::Isotropic::Sigma(1, 1.)));
graph.push_back(
PriorFactor<Pose3>(X(1), Pose3(Rot3(), Point3(1., 0., 0.)),
noiseModel::Isotropic::Sigma(6, 1.)));
graph.emplace_shared<
PriorFactor<CalibratedCamera> >(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.)),
noiseModel::Isotropic::Sigma(6, 1.));
Values init;
init.insert(X(0), CalibratedCamera());

6
gtsam/slam/tests/testGeneralSFMFactor_Cal3Bundler.cpp
View File

@ -369,9 +369,9 @@ TEST( GeneralSFMFactor_Cal3Bundler, optimize_varK_BA ) {
graph.addCameraConstraint(0, cameras[0]);
// Constrain the scale of the problem with a soft range factor of 1m between the cameras
graph.push_back(
RangeFactor<GeneralCamera, GeneralCamera>(X(0), X(1), 2.,
noiseModel::Isotropic::Sigma(1, 10.)));
graph.emplace_shared<
RangeFactor<GeneralCamera, GeneralCamera> >(X(0), X(1), 2.,
noiseModel::Isotropic::Sigma(1, 10.));
const double reproj_error = 1e-5;

24
gtsam/slam/tests/testReferenceFrameFactor.cpp
View File

@ -136,15 +136,15 @@ TEST( ReferenceFrameFactor, converge_trans ) {
// Pose2 trans = transIdeal * Pose2(-200.0, 100.0, 2.0); // beyond pi/2 - fails
NonlinearFactorGraph graph;
graph.push_back(PointReferenceFrameFactor(lB1, tA1, lA1));
graph.push_back(PointReferenceFrameFactor(lB2, tA1, lA2));
graph.emplace_shared<PointReferenceFrameFactor>(lB1, tA1, lA1);
graph.emplace_shared<PointReferenceFrameFactor>(lB2, tA1, lA2);
// hard constraints on points
double error_gain = 1000.0;
graph.push_back(NonlinearEquality<gtsam::Point2>(lA1, local1, error_gain));
graph.push_back(NonlinearEquality<gtsam::Point2>(lA2, local2, error_gain));
graph.push_back(NonlinearEquality<gtsam::Point2>(lB1, global1, error_gain));
graph.push_back(NonlinearEquality<gtsam::Point2>(lB2, global2, error_gain));
graph.emplace_shared<NonlinearEquality<gtsam::Point2> >(lA1, local1, error_gain);
graph.emplace_shared<NonlinearEquality<gtsam::Point2> >(lA2, local2, error_gain);
graph.emplace_shared<NonlinearEquality<gtsam::Point2> >(lB1, global1, error_gain);
graph.emplace_shared<NonlinearEquality<gtsam::Point2> >(lB2, global2, error_gain);
// create initial estimate
Values init;
@ -186,9 +186,9 @@ TEST( ReferenceFrameFactor, converge_local ) {
NonlinearFactorGraph graph;
double error_gain = 1000.0;
graph.push_back(PointReferenceFrameFactor(lB1, tA1, lA1));
graph.push_back(NonlinearEquality<gtsam::Point2>(lB1, global, error_gain));
graph.push_back(NonlinearEquality<gtsam::Pose2>(tA1, trans, error_gain));
graph.emplace_shared<PointReferenceFrameFactor>(lB1, tA1, lA1);
graph.emplace_shared<NonlinearEquality<gtsam::Point2> >(lB1, global, error_gain);
graph.emplace_shared<NonlinearEquality<gtsam::Pose2> >(tA1, trans, error_gain);
// create initial estimate
Values init;
@ -222,9 +222,9 @@ TEST( ReferenceFrameFactor, converge_global ) {
NonlinearFactorGraph graph;
double error_gain = 1000.0;
graph.push_back(PointReferenceFrameFactor(lB1, tA1, lA1));
graph.push_back(NonlinearEquality<gtsam::Point2>(lA1, local, error_gain));
graph.push_back(NonlinearEquality<gtsam::Pose2>(tA1, trans, error_gain));
graph.emplace_shared<PointReferenceFrameFactor>(lB1, tA1, lA1);
graph.emplace_shared<NonlinearEquality<gtsam::Point2> >(lA1, local, error_gain);
graph.emplace_shared<NonlinearEquality<gtsam::Pose2> >(tA1, trans, error_gain);
// create initial estimate
Values init;

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

@ -209,8 +209,8 @@ TEST( SmartProjectionCameraFactor, perturbPoseAndOptimize ) {
graph.push_back(smartFactor2);
graph.push_back(smartFactor3);
const SharedDiagonal noisePrior = noiseModel::Isotropic::Sigma(6 + 5, 1e-5);
graph.push_back(PriorFactor<Camera>(c1, cam1, noisePrior));
graph.push_back(PriorFactor<Camera>(c2, cam2, noisePrior));
graph.emplace_shared<PriorFactor<Camera> >(c1, cam1, noisePrior);
graph.emplace_shared<PriorFactor<Camera> >(c2, cam2, noisePrior);
// Create initial estimate
Values initial;
@ -321,8 +321,8 @@ TEST( SmartProjectionCameraFactor, perturbPoseAndOptimizeFromSfM_tracks ) {
graph.push_back(smartFactor1);
graph.push_back(smartFactor2);
graph.push_back(smartFactor3);
graph.push_back(PriorFactor<Camera>(c1, cam1, noisePrior));
graph.push_back(PriorFactor<Camera>(c2, cam2, noisePrior));
graph.emplace_shared<PriorFactor<Camera> >(c1, cam1, noisePrior);
graph.emplace_shared<PriorFactor<Camera> >(c2, cam2, noisePrior);
Values values;
values.insert(c1, cam1);
@ -398,8 +398,8 @@ TEST( SmartProjectionCameraFactor, perturbCamerasAndOptimize ) {
graph.push_back(smartFactor3);
graph.push_back(smartFactor4);
graph.push_back(smartFactor5);
graph.push_back(PriorFactor<Camera>(c1, cam1, noisePrior));
graph.push_back(PriorFactor<Camera>(c2, cam2, noisePrior));
graph.emplace_shared<PriorFactor<Camera> >(c1, cam1, noisePrior);
graph.emplace_shared<PriorFactor<Camera> >(c2, cam2, noisePrior);
Values values;
values.insert(c1, cam1);
@ -476,8 +476,8 @@ TEST( SmartProjectionCameraFactor, Cal3Bundler ) {
graph.push_back(smartFactor1);
graph.push_back(smartFactor2);
graph.push_back(smartFactor3);
graph.push_back(PriorFactor<Camera>(c1, cam1, noisePrior));
graph.push_back(PriorFactor<Camera>(c2, cam2, noisePrior));
graph.emplace_shared<PriorFactor<Camera> >(c1, cam1, noisePrior);
graph.emplace_shared<PriorFactor<Camera> >(c2, cam2, noisePrior);
Values values;
values.insert(c1, cam1);
@ -552,8 +552,8 @@ TEST( SmartProjectionCameraFactor, Cal3Bundler2 ) {
graph.push_back(smartFactor1);
graph.push_back(smartFactor2);
graph.push_back(smartFactor3);
graph.push_back(PriorFactor<Camera>(c1, cam1, noisePrior));
graph.push_back(PriorFactor<Camera>(c2, cam2, noisePrior));
graph.emplace_shared<PriorFactor<Camera> >(c1, cam1, noisePrior);
graph.emplace_shared<PriorFactor<Camera> >(c2, cam2, noisePrior);
Values values;
values.insert(c1, cam1);

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

@ -262,8 +262,8 @@ TEST( SmartProjectionPoseFactor, smartFactorWithSensorBodyTransform ){
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.emplace_shared<PriorFactor<Pose3> >(x1, bodyPose1, noisePrior);
graph.emplace_shared<PriorFactor<Pose3> >(x2, bodyPose2, noisePrior);
// Check errors at ground truth poses
Values gtValues;
@ -319,8 +319,8 @@ TEST( SmartProjectionPoseFactor, 3poses_smart_projection_factor ) {
graph.push_back(smartFactor1);
graph.push_back(smartFactor2);
graph.push_back(smartFactor3);
graph.push_back(PriorFactor<Pose3>(x1, cam1.pose(), noisePrior));
graph.push_back(PriorFactor<Pose3>(x2, cam2.pose(), noisePrior));
graph.emplace_shared<PriorFactor<Pose3> >(x1, cam1.pose(), noisePrior);
graph.emplace_shared<PriorFactor<Pose3> >(x2, cam2.pose(), noisePrior);
Values groundTruth;
groundTruth.insert(x1, cam1.pose());
@ -547,8 +547,8 @@ TEST( SmartProjectionPoseFactor, 3poses_iterative_smart_projection_factor ) {
graph.push_back(smartFactor1);
graph.push_back(smartFactor2);
graph.push_back(smartFactor3);
graph.push_back(PriorFactor<Pose3>(x1, cam1.pose(), noisePrior));
graph.push_back(PriorFactor<Pose3>(x2, cam2.pose(), noisePrior));
graph.emplace_shared<PriorFactor<Pose3> >(x1, cam1.pose(), noisePrior);
graph.emplace_shared<PriorFactor<Pose3> >(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),
@ -614,8 +614,8 @@ TEST( SmartProjectionPoseFactor, jacobianSVD ) {
graph.push_back(smartFactor1);
graph.push_back(smartFactor2);
graph.push_back(smartFactor3);
graph.push_back(PriorFactor<Pose3>(x1, cam1.pose(), noisePrior));
graph.push_back(PriorFactor<Pose3>(x2, cam2.pose(), noisePrior));
graph.emplace_shared<PriorFactor<Pose3> >(x1, cam1.pose(), noisePrior);
graph.emplace_shared<PriorFactor<Pose3> >(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),
@ -675,8 +675,8 @@ TEST( SmartProjectionPoseFactor, landmarkDistance ) {
graph.push_back(smartFactor1);
graph.push_back(smartFactor2);
graph.push_back(smartFactor3);
graph.push_back(PriorFactor<Pose3>(x1, cam1.pose(), noisePrior));
graph.push_back(PriorFactor<Pose3>(x2, cam2.pose(), noisePrior));
graph.emplace_shared<PriorFactor<Pose3> >(x1, cam1.pose(), noisePrior);
graph.emplace_shared<PriorFactor<Pose3> >(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),
@ -747,8 +747,8 @@ TEST( SmartProjectionPoseFactor, dynamicOutlierRejection ) {
graph.push_back(smartFactor2);
graph.push_back(smartFactor3);
graph.push_back(smartFactor4);
graph.push_back(PriorFactor<Pose3>(x1, cam1.pose(), noisePrior));
graph.push_back(PriorFactor<Pose3>(x2, cam2.pose(), noisePrior));
graph.emplace_shared<PriorFactor<Pose3> >(x1, cam1.pose(), noisePrior);
graph.emplace_shared<PriorFactor<Pose3> >(x2, cam2.pose(), noisePrior);
Values values;
values.insert(x1, cam1.pose());
@ -800,8 +800,8 @@ TEST( SmartProjectionPoseFactor, jacobianQ ) {
graph.push_back(smartFactor1);
graph.push_back(smartFactor2);
graph.push_back(smartFactor3);
graph.push_back(PriorFactor<Pose3>(x1, cam1.pose(), noisePrior));
graph.push_back(PriorFactor<Pose3>(x2, cam2.pose(), noisePrior));
graph.emplace_shared<PriorFactor<Pose3> >(x1, cam1.pose(), noisePrior);
graph.emplace_shared<PriorFactor<Pose3> >(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
@ -830,30 +830,21 @@ TEST( SmartProjectionPoseFactor, 3poses_projection_factor ) {
NonlinearFactorGraph graph;
// Project three landmarks into three cameras
graph.push_back(
ProjectionFactor(cam1.project(landmark1), model, x1, L(1), sharedK2));
graph.push_back(
ProjectionFactor(cam2.project(landmark1), model, x2, L(1), sharedK2));
graph.push_back(
ProjectionFactor(cam3.project(landmark1), model, x3, L(1), sharedK2));
graph.emplace_shared<ProjectionFactor>(cam1.project(landmark1), model, x1, L(1), sharedK2);
graph.emplace_shared<ProjectionFactor>(cam2.project(landmark1), model, x2, L(1), sharedK2);
graph.emplace_shared<ProjectionFactor>(cam3.project(landmark1), model, x3, L(1), sharedK2);
graph.push_back(
ProjectionFactor(cam1.project(landmark2), model, x1, L(2), sharedK2));
graph.push_back(
ProjectionFactor(cam2.project(landmark2), model, x2, L(2), sharedK2));
graph.push_back(
ProjectionFactor(cam3.project(landmark2), model, x3, L(2), sharedK2));
graph.emplace_shared<ProjectionFactor>(cam1.project(landmark2), model, x1, L(2), sharedK2);
graph.emplace_shared<ProjectionFactor>(cam2.project(landmark2), model, x2, L(2), sharedK2);
graph.emplace_shared<ProjectionFactor>(cam3.project(landmark2), model, x3, L(2), sharedK2);
graph.push_back(
ProjectionFactor(cam1.project(landmark3), model, x1, L(3), sharedK2));
graph.push_back(
ProjectionFactor(cam2.project(landmark3), model, x2, L(3), sharedK2));
graph.push_back(
ProjectionFactor(cam3.project(landmark3), model, x3, L(3), sharedK2));
graph.emplace_shared<ProjectionFactor>(cam1.project(landmark3), model, x1, L(3), sharedK2);
graph.emplace_shared<ProjectionFactor>(cam2.project(landmark3), model, x2, L(3), sharedK2);
graph.emplace_shared<ProjectionFactor>(cam3.project(landmark3), model, x3, L(3), sharedK2);
const SharedDiagonal noisePrior = noiseModel::Isotropic::Sigma(6, 0.10);
graph.push_back(PriorFactor<Pose3>(x1, level_pose, noisePrior));
graph.push_back(PriorFactor<Pose3>(x2, pose_right, noisePrior));
graph.emplace_shared<PriorFactor<Pose3> >(x1, level_pose, noisePrior);
graph.emplace_shared<PriorFactor<Pose3> >(x2, pose_right, noisePrior);
Pose3 noise_pose = Pose3(Rot3::Ypr(-M_PI / 10, 0., -M_PI / 10),
Point3(0.5, 0.1, 0.3));
@ -1000,11 +991,9 @@ TEST( SmartProjectionPoseFactor, 3poses_2land_rotation_only_smart_projection_fac
NonlinearFactorGraph graph;
graph.push_back(smartFactor1);
graph.push_back(smartFactor2);
graph.push_back(PriorFactor<Pose3>(x1, cam1.pose(), noisePrior));
graph.push_back(
PoseTranslationPrior<Pose3>(x2, positionPrior, noisePriorTranslation));
graph.push_back(
PoseTranslationPrior<Pose3>(x3, positionPrior, noisePriorTranslation));
graph.emplace_shared<PriorFactor<Pose3> >(x1, cam1.pose(), noisePrior);
graph.emplace_shared<PoseTranslationPrior<Pose3> >(x2, positionPrior, noisePriorTranslation);
graph.emplace_shared<PoseTranslationPrior<Pose3> >(x3, positionPrior, noisePriorTranslation);
Pose3 noise_pose = Pose3(Rot3::Ypr(-M_PI / 100, 0., -M_PI / 100),
Point3(0.1, 0.1, 0.1)); // smaller noise
@ -1068,11 +1057,9 @@ TEST( SmartProjectionPoseFactor, 3poses_rotation_only_smart_projection_factor )
graph.push_back(smartFactor1);
graph.push_back(smartFactor2);
graph.push_back(smartFactor3);
graph.push_back(PriorFactor<Pose3>(x1, cam1.pose(), noisePrior));
graph.push_back(
PoseTranslationPrior<Pose3>(x2, positionPrior, noisePriorTranslation));
graph.push_back(
PoseTranslationPrior<Pose3>(x3, positionPrior, noisePriorTranslation));
graph.emplace_shared<PriorFactor<Pose3> >(x1, cam1.pose(), noisePrior);
graph.emplace_shared<PoseTranslationPrior<Pose3> >(x2, positionPrior, noisePriorTranslation);
graph.emplace_shared<PoseTranslationPrior<Pose3> >(x3, positionPrior, noisePriorTranslation);
// 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),
@ -1286,8 +1273,8 @@ TEST( SmartProjectionPoseFactor, Cal3Bundler ) {
graph.push_back(smartFactor1);
graph.push_back(smartFactor2);
graph.push_back(smartFactor3);
graph.push_back(PriorFactor<Pose3>(x1, cam1.pose(), noisePrior));
graph.push_back(PriorFactor<Pose3>(x2, cam2.pose(), noisePrior));
graph.emplace_shared<PriorFactor<Pose3> >(x1, cam1.pose(), noisePrior);
graph.emplace_shared<PriorFactor<Pose3> >(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),
@ -1362,11 +1349,9 @@ TEST( SmartProjectionPoseFactor, Cal3BundlerRotationOnly ) {
graph.push_back(smartFactor1);
graph.push_back(smartFactor2);
graph.push_back(smartFactor3);
graph.push_back(PriorFactor<Pose3>(x1, cam1.pose(), noisePrior));
graph.push_back(
PoseTranslationPrior<Pose3>(x2, positionPrior, noisePriorTranslation));
graph.push_back(
PoseTranslationPrior<Pose3>(x3, positionPrior, noisePriorTranslation));
graph.emplace_shared<PriorFactor<Pose3> >(x1, cam1.pose(), noisePrior);
graph.emplace_shared<PoseTranslationPrior<Pose3> >(x2, positionPrior, noisePriorTranslation);
graph.emplace_shared<PoseTranslationPrior<Pose3> >(x3, positionPrior, noisePriorTranslation);
// 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),

4
gtsam/slam/tests/testStereoFactor.cpp
View File

@ -185,11 +185,11 @@ TEST( StereoFactor, singlePoint)
{
NonlinearFactorGraph graph;
graph.push_back(NonlinearEquality<Pose3>(X(1), camera1));
graph.emplace_shared<NonlinearEquality<Pose3> >(X(1), camera1);
StereoPoint2 measurement(320, 320.0-50, 240);
// arguments: measurement, sigma, cam#, measurement #, K, baseline (m)
graph.push_back(GenericStereoFactor<Pose3, Point3>(measurement, model, X(1), L(1), K));
graph.emplace_shared<GenericStereoFactor<Pose3, Point3> >(measurement, model, X(1), L(1), K);
// Create a configuration corresponding to the ground truth
Values values;

10
gtsam_unstable/examples/ConcurrentCalibration.cpp
View File

@ -61,7 +61,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.push_back(PriorFactor<Cal3_S2>(Symbol('K', 0), *noisy_K, calNoise));
graph.emplace_shared<PriorFactor<Cal3_S2> >(Symbol('K', 0), *noisy_K, calNoise);
ifstream pose_file(pose_loc.c_str());
@ -77,7 +77,7 @@ int main(int argc, char** argv){
}
noiseModel::Isotropic::shared_ptr poseNoise = noiseModel::Isotropic::Sigma(6, 0.01);
graph.push_back(PriorFactor<Pose3>(Symbol('x', pose_id), Pose3(m), poseNoise));
graph.emplace_shared<PriorFactor<Pose3> >(Symbol('x', pose_id), Pose3(m), poseNoise);
// camera and landmark keys
size_t x, l;
@ -89,9 +89,9 @@ int main(int argc, char** argv){
cout << "Reading stereo factors" << endl;
//read stereo measurement details from file and use to create and add GenericStereoFactor objects to the graph representation
while (factor_file >> x >> l >> uL >> uR >> v >> X >> Y >> Z) {
// graph.push_back( GenericStereoFactor<Pose3, Point3>(StereoPoint2(uL, uR, v), model, Symbol('x', x), Symbol('l', l), K));
// graph.emplace_shared<GenericStereoFactor<Pose3, Point3> >(StereoPoint2(uL, uR, v), model, Symbol('x', x), Symbol('l', l), K);
graph.push_back(GeneralSFMFactor2<Cal3_S2>(Point2(uL,v), model, Symbol('x', x), Symbol('l', l), Symbol('K', 0)));
graph.emplace_shared<GeneralSFMFactor2<Cal3_S2> >(Point2(uL,v), model, Symbol('x', x), Symbol('l', l), Symbol('K', 0));
//if the landmark variable included in this factor has not yet been added to the initial variable value estimate, add it
@ -107,7 +107,7 @@ int main(int argc, char** argv){
//constrain the first pose such that it cannot change from its original value during optimization
// NOTE: NonlinearEquality forces the optimizer to use QR rather than Cholesky
// QR is much slower than Cholesky, but numerically more stable
graph.push_back(NonlinearEquality<Pose3>(Symbol('x',1),first_pose));
graph.emplace_shared<NonlinearEquality<Pose3> >(Symbol('x',1),first_pose);
cout << "Optimizing" << endl;
LevenbergMarquardtParams params;

2
gtsam_unstable/examples/SmartProjectionFactorExample.cpp
View File

@ -104,7 +104,7 @@ int main(int argc, char** argv){
//constrain the first pose such that it cannot change from its original value during optimization
// NOTE: NonlinearEquality forces the optimizer to use QR rather than Cholesky
// QR is much slower than Cholesky, but numerically more stable
graph.push_back(NonlinearEquality<Pose3>(1,firstPose));
graph.emplace_shared<NonlinearEquality<Pose3> >(1,firstPose);
LevenbergMarquardtParams params;
params.verbosityLM = LevenbergMarquardtParams::TRYLAMBDA;

2
gtsam_unstable/examples/SmartStereoProjectionFactorExample.cpp
View File

@ -126,7 +126,7 @@ int main(int argc, char** argv){
//constrain the first pose such that it cannot change from its original value during optimization
// NOTE: NonlinearEquality forces the optimizer to use QR rather than Cholesky
// QR is much slower than Cholesky, but numerically more stable
graph.push_back(NonlinearEquality<Pose3>(Symbol('x',1),first_pose));
graph.emplace_shared<NonlinearEquality<Pose3> >(Symbol('x',1),first_pose);
LevenbergMarquardtParams params;
params.verbosityLM = LevenbergMarquardtParams::TRYLAMBDA;

7
gtsam_unstable/linear/LPSolver.h
View File

@ -51,7 +51,7 @@ struct LPPolicy {
++it) {
size_t dim = lp.cost.getDim(it);
Vector b = xk.at(*it) - lp.cost.getA(it).transpose(); // b = xk-g
graph.push_back(JacobianFactor(*it, Matrix::Identity(dim, dim), b));
graph.emplace_shared<JacobianFactor>(*it, Matrix::Identity(dim, dim), b);
}
KeySet allKeys = lp.inequalities.keys();
@ -67,8 +67,7 @@ struct LPPolicy {
std::inserter(difference, difference.end()));
for (Key k : difference) {
size_t dim = lp.constrainedKeyDimMap().at(k);
graph.push_back(
JacobianFactor(k, Matrix::Identity(dim, dim), xk.at(k)));
graph.emplace_shared<JacobianFactor>(k, Matrix::Identity(dim, dim), xk.at(k));
}
}
return graph;
@ -77,4 +76,4 @@ struct LPPolicy {
using LPSolver = ActiveSetSolver<LP, LPPolicy, LPInitSolver>;
}
}

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

@ -403,9 +403,9 @@ TEST( ConcurrentBatchFilter, update_and_marginalize )
NonlinearFactorGraph partialGraph;
partialGraph.push_back(PriorFactor<Pose3>(1, poseInitial, noisePrior));
partialGraph.push_back(BetweenFactor<Pose3>(1, 2, poseOdometry, noiseOdometery));
partialGraph.push_back(BetweenFactor<Pose3>(2, 3, poseOdometry, noiseOdometery));
partialGraph.emplace_shared<PriorFactor<Pose3> >(1, poseInitial, noisePrior);
partialGraph.emplace_shared<BetweenFactor<Pose3> >(1, 2, poseOdometry, noiseOdometery);
partialGraph.emplace_shared<BetweenFactor<Pose3> >(2, 3, poseOdometry, noiseOdometery);
Values partialValues;
partialValues.insert(1, optimalValues.at<Pose3>(1));
@ -437,10 +437,10 @@ TEST( ConcurrentBatchFilter, update_and_marginalize )
expectedGraph.push_back(NonlinearFactor::shared_ptr());
expectedGraph.push_back(NonlinearFactor::shared_ptr());
// ==========================================================
expectedGraph.push_back(BetweenFactor<Pose3>(3, 4, poseOdometry, noiseOdometery));
expectedGraph.emplace_shared<BetweenFactor<Pose3> >(3, 4, poseOdometry, noiseOdometery);
for(const GaussianFactor::shared_ptr& factor: result) {
expectedGraph.push_back(LinearContainerFactor(factor, partialValues));
expectedGraph.emplace_shared<LinearContainerFactor>(factor, partialValues);
}
@ -1126,13 +1126,13 @@ TEST( ConcurrentBatchFilter, removeFactors_topology_1 )
NonlinearFactorGraph actualGraph = filter.getFactors();
NonlinearFactorGraph expectedGraph;
expectedGraph.push_back(PriorFactor<Pose3>(1, poseInitial, noisePrior));
// we removed this one: expectedGraph.push_back(BetweenFactor<Pose3>(1, 2, poseOdometry, noiseOdometery));
expectedGraph.emplace_shared<PriorFactor<Pose3> >(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());
expectedGraph.push_back(BetweenFactor<Pose3>(2, 3, poseOdometry, noiseOdometery));
expectedGraph.push_back(BetweenFactor<Pose3>(3, 4, poseOdometry, noiseOdometery));
expectedGraph.push_back(BetweenFactor<Pose3>(1, 2, poseOdometry, noiseOdometery));
expectedGraph.emplace_shared<BetweenFactor<Pose3> >(2, 3, poseOdometry, noiseOdometery);
expectedGraph.emplace_shared<BetweenFactor<Pose3> >(3, 4, poseOdometry, noiseOdometery);
expectedGraph.emplace_shared<BetweenFactor<Pose3> >(1, 2, poseOdometry, noiseOdometery);
CHECK(assert_equal(expectedGraph, actualGraph, 1e-6));
}
@ -1181,11 +1181,11 @@ TEST( ConcurrentBatchFilter, removeFactors_topology_2 )
NonlinearFactorGraph actualGraph = filter.getFactors();
NonlinearFactorGraph expectedGraph;
expectedGraph.push_back(PriorFactor<Pose3>(1, poseInitial, noisePrior));
expectedGraph.push_back(BetweenFactor<Pose3>(1, 2, poseOdometry, noiseOdometery));
expectedGraph.push_back(BetweenFactor<Pose3>(2, 3, poseOdometry, noiseOdometery));
expectedGraph.push_back(BetweenFactor<Pose3>(3, 4, poseOdometry, noiseOdometery));
// we removed this one: expectedGraph.push_back(BetweenFactor<Pose3>(1, 2, poseOdometry, noiseOdometery));
expectedGraph.emplace_shared<PriorFactor<Pose3> >(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);
// 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());
@ -1238,10 +1238,10 @@ 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.push_back(PriorFactor<Pose3>(1, poseInitial, noisePrior));
expectedGraph.push_back(BetweenFactor<Pose3>(1, 2, poseOdometry, noiseOdometery));
expectedGraph.push_back(BetweenFactor<Pose3>(2, 3, poseOdometry, noiseOdometery));
expectedGraph.push_back(BetweenFactor<Pose3>(3, 4, poseOdometry, noiseOdometery));
expectedGraph.emplace_shared<PriorFactor<Pose3> >(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);
CHECK(assert_equal(expectedGraph, actualGraph, 1e-6));
}
@ -1258,11 +1258,11 @@ TEST( ConcurrentBatchFilter, removeFactors_values )
// Add some factors to the filter
NonlinearFactorGraph newFactors;
newFactors.push_back(PriorFactor<Pose3>(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));
newFactors.push_back(BetweenFactor<Pose3>(1, 2, poseOdometry, noiseOdometery));
newFactors.emplace_shared<PriorFactor<Pose3> >(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);
newFactors.emplace_shared<BetweenFactor<Pose3> >(1, 2, poseOdometry, noiseOdometery);
Values newValues;
newValues.insert(1, Pose3().compose(poseError));
@ -1291,11 +1291,11 @@ TEST( ConcurrentBatchFilter, removeFactors_values )
// note: factors are removed before the optimization
NonlinearFactorGraph expectedGraph;
expectedGraph.push_back(PriorFactor<Pose3>(1, poseInitial, noisePrior));
expectedGraph.push_back(BetweenFactor<Pose3>(1, 2, poseOdometry, noiseOdometery));
expectedGraph.push_back(BetweenFactor<Pose3>(2, 3, poseOdometry, noiseOdometery));
expectedGraph.push_back(BetweenFactor<Pose3>(3, 4, poseOdometry, noiseOdometery));
// we removed this one: expectedGraph.push_back(BetweenFactor<Pose3>(1, 2, poseOdometry, noiseOdometery));
expectedGraph.emplace_shared<PriorFactor<Pose3> >(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);
// 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());

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

@ -617,13 +617,13 @@ TEST( ConcurrentBatchSmoother, removeFactors_topology_1 )
NonlinearFactorGraph actualGraph = smoother.getFactors();
NonlinearFactorGraph expectedGraph;
expectedGraph.push_back(PriorFactor<Pose3>(1, poseInitial, noisePrior));
// we removed this one: expectedGraph.push_back(BetweenFactor<Pose3>(1, 2, poseOdometry, noiseOdometery));
expectedGraph.emplace_shared<PriorFactor<Pose3> >(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());
expectedGraph.push_back(BetweenFactor<Pose3>(2, 3, poseOdometry, noiseOdometery));
expectedGraph.push_back(BetweenFactor<Pose3>(3, 4, poseOdometry, noiseOdometery));
expectedGraph.push_back(BetweenFactor<Pose3>(1, 2, poseOdometry, noiseOdometery));
expectedGraph.emplace_shared<BetweenFactor<Pose3> >(2, 3, poseOdometry, noiseOdometery);
expectedGraph.emplace_shared<BetweenFactor<Pose3> >(3, 4, poseOdometry, noiseOdometery);
expectedGraph.emplace_shared<BetweenFactor<Pose3> >(1, 2, poseOdometry, noiseOdometery);
CHECK(assert_equal(expectedGraph, actualGraph, 1e-6));
}
@ -670,11 +670,11 @@ TEST( ConcurrentBatchSmoother, removeFactors_topology_2 )
NonlinearFactorGraph actualGraph = smoother.getFactors();
NonlinearFactorGraph expectedGraph;
expectedGraph.push_back(PriorFactor<Pose3>(1, poseInitial, noisePrior));
expectedGraph.push_back(BetweenFactor<Pose3>(1, 2, poseOdometry, noiseOdometery));
expectedGraph.push_back(BetweenFactor<Pose3>(2, 3, poseOdometry, noiseOdometery));
expectedGraph.push_back(BetweenFactor<Pose3>(3, 4, poseOdometry, noiseOdometery));
// we removed this one: expectedGraph.push_back(BetweenFactor<Pose3>(1, 2, poseOdometry, noiseOdometery));
expectedGraph.emplace_shared<PriorFactor<Pose3> >(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);
// 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());
@ -724,10 +724,10 @@ 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.push_back(PriorFactor<Pose3>(1, poseInitial, noisePrior));
expectedGraph.push_back(BetweenFactor<Pose3>(1, 2, poseOdometry, noiseOdometery));
expectedGraph.push_back(BetweenFactor<Pose3>(2, 3, poseOdometry, noiseOdometery));
expectedGraph.push_back(BetweenFactor<Pose3>(3, 4, poseOdometry, noiseOdometery));
expectedGraph.emplace_shared<PriorFactor<Pose3> >(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);
CHECK(assert_equal(expectedGraph, actualGraph, 1e-6));
}
@ -744,11 +744,11 @@ TEST( ConcurrentBatchSmoother, removeFactors_values )
// Add some factors to the Smoother
NonlinearFactorGraph newFactors;
newFactors.push_back(PriorFactor<Pose3>(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));
newFactors.push_back(BetweenFactor<Pose3>(1, 2, poseOdometry, noiseOdometery));
newFactors.emplace_shared<PriorFactor<Pose3> >(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);
newFactors.emplace_shared<BetweenFactor<Pose3> >(1, 2, poseOdometry, noiseOdometery);
Values newValues;
newValues.insert(1, Pose3().compose(poseError));
@ -774,11 +774,11 @@ TEST( ConcurrentBatchSmoother, removeFactors_values )
// note: factors are removed before the optimization
NonlinearFactorGraph expectedGraph;
expectedGraph.push_back(PriorFactor<Pose3>(1, poseInitial, noisePrior));
expectedGraph.push_back(BetweenFactor<Pose3>(1, 2, poseOdometry, noiseOdometery));
expectedGraph.push_back(BetweenFactor<Pose3>(2, 3, poseOdometry, noiseOdometery));
expectedGraph.push_back(BetweenFactor<Pose3>(3, 4, poseOdometry, noiseOdometery));
// we removed this one: expectedGraph.push_back(BetweenFactor<Pose3>(1, 2, poseOdometry, noiseOdometery));
expectedGraph.emplace_shared<PriorFactor<Pose3> >(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);
// 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());

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

@ -423,9 +423,9 @@ TEST( ConcurrentIncrementalFilter, update_and_marginalize_1 )
// ----------------------------------------------------------------------------------------------
NonlinearFactorGraph partialGraph;
partialGraph.push_back(PriorFactor<Pose3>(1, poseInitial, noisePrior));
partialGraph.push_back(BetweenFactor<Pose3>(1, 2, poseOdometry, noiseOdometery));
partialGraph.push_back(BetweenFactor<Pose3>(2, 3, poseOdometry, noiseOdometery));
partialGraph.emplace_shared<PriorFactor<Pose3> >(1, poseInitial, noisePrior);
partialGraph.emplace_shared<BetweenFactor<Pose3> >(1, 2, poseOdometry, noiseOdometery);
partialGraph.emplace_shared<BetweenFactor<Pose3> >(2, 3, poseOdometry, noiseOdometery);
GaussianFactorGraph linearGraph = *partialGraph.linearize(newValues);
@ -441,7 +441,7 @@ TEST( ConcurrentIncrementalFilter, update_and_marginalize_1 )
expectedGraph.push_back(NonlinearFactor::shared_ptr());
expectedGraph.push_back(NonlinearFactor::shared_ptr());
// ==========================================================
expectedGraph.push_back(BetweenFactor<Pose3>(3, 4, poseOdometry, noiseOdometery));
expectedGraph.emplace_shared<BetweenFactor<Pose3> >(3, 4, poseOdometry, noiseOdometery);
for(const GaussianFactor::shared_ptr& factor: marginal) {
// the linearization point for the linear container is optional, but it is not used in the filter,
@ -507,9 +507,9 @@ TEST( ConcurrentIncrementalFilter, update_and_marginalize_2 )
// ----------------------------------------------------------------------------------------------
NonlinearFactorGraph partialGraph;
partialGraph.push_back(PriorFactor<Pose3>(1, poseInitial, noisePrior));
partialGraph.push_back(BetweenFactor<Pose3>(1, 2, poseOdometry, noiseOdometery));
partialGraph.push_back(BetweenFactor<Pose3>(2, 3, poseOdometry, noiseOdometery));
partialGraph.emplace_shared<PriorFactor<Pose3> >(1, poseInitial, noisePrior);
partialGraph.emplace_shared<BetweenFactor<Pose3> >(1, 2, poseOdometry, noiseOdometery);
partialGraph.emplace_shared<BetweenFactor<Pose3> >(2, 3, poseOdometry, noiseOdometery);
GaussianFactorGraph linearGraph = *partialGraph.linearize(optimalValues);
@ -525,7 +525,7 @@ TEST( ConcurrentIncrementalFilter, update_and_marginalize_2 )
expectedGraph.push_back(NonlinearFactor::shared_ptr());
expectedGraph.push_back(NonlinearFactor::shared_ptr());
// ==========================================================
expectedGraph.push_back(BetweenFactor<Pose3>(3, 4, poseOdometry, noiseOdometery));
expectedGraph.emplace_shared<BetweenFactor<Pose3> >(3, 4, poseOdometry, noiseOdometery);
for(const GaussianFactor::shared_ptr& factor: marginal) {
// the linearization point for the linear container is optional, but it is not used in the filter,
@ -1231,13 +1231,13 @@ TEST( ConcurrentIncrementalFilter, removeFactors_topology_1 )
NonlinearFactorGraph actualGraph = filter.getFactors();
NonlinearFactorGraph expectedGraph;
expectedGraph.push_back(PriorFactor<Pose3>(1, poseInitial, noisePrior));
expectedGraph.emplace_shared<PriorFactor<Pose3> >(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());
expectedGraph.push_back(BetweenFactor<Pose3>(2, 3, poseOdometry, noiseOdometery));
expectedGraph.push_back(BetweenFactor<Pose3>(3, 4, poseOdometry, noiseOdometery));
expectedGraph.push_back(BetweenFactor<Pose3>(1, 2, poseOdometry, noiseOdometery));
expectedGraph.emplace_shared<BetweenFactor<Pose3> >(2, 3, poseOdometry, noiseOdometery);
expectedGraph.emplace_shared<BetweenFactor<Pose3> >(3, 4, poseOdometry, noiseOdometery);
expectedGraph.emplace_shared<BetweenFactor<Pose3> >(1, 2, poseOdometry, noiseOdometery);
CHECK(assert_equal(expectedGraph, actualGraph, 1e-6));
}
@ -1289,11 +1289,11 @@ TEST( ConcurrentIncrementalFilter, removeFactors_topology_2 )
NonlinearFactorGraph actualGraph = filter.getFactors();
NonlinearFactorGraph expectedGraph;
expectedGraph.push_back(PriorFactor<Pose3>(1, poseInitial, noisePrior));
expectedGraph.push_back(BetweenFactor<Pose3>(1, 2, poseOdometry, noiseOdometery));
expectedGraph.push_back(BetweenFactor<Pose3>(2, 3, poseOdometry, noiseOdometery));
expectedGraph.push_back(BetweenFactor<Pose3>(3, 4, poseOdometry, noiseOdometery));
// we removed this one: expectedGraph.push_back(BetweenFactor<Pose3>(1, 2, poseOdometry, noiseOdometery));
expectedGraph.emplace_shared<PriorFactor<Pose3> >(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);
// 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());
@ -1350,10 +1350,10 @@ 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.push_back(PriorFactor<Pose3>(1, poseInitial, noisePrior));
expectedGraph.push_back(BetweenFactor<Pose3>(1, 2, poseOdometry, noiseOdometery));
expectedGraph.push_back(BetweenFactor<Pose3>(2, 3, poseOdometry, noiseOdometery));
expectedGraph.push_back(BetweenFactor<Pose3>(3, 4, poseOdometry, noiseOdometery));
expectedGraph.emplace_shared<PriorFactor<Pose3> >(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);
CHECK(assert_equal(expectedGraph, actualGraph, 1e-6));
}
@ -1406,11 +1406,11 @@ TEST( ConcurrentIncrementalFilter, removeFactors_values )
Values actualValues = filter.getLinearizationPoint();
NonlinearFactorGraph expectedGraph;
expectedGraph.push_back(PriorFactor<Pose3>(1, poseInitial, noisePrior));
expectedGraph.push_back(BetweenFactor<Pose3>(1, 2, poseOdometry, noiseOdometery));
expectedGraph.push_back(BetweenFactor<Pose3>(2, 3, poseOdometry, noiseOdometery));
expectedGraph.push_back(BetweenFactor<Pose3>(3, 4, poseOdometry, noiseOdometery));
// we removed this one: expectedGraph.push_back(BetweenFactor<Pose3>(1, 2, poseOdometry, noiseOdometery));
expectedGraph.emplace_shared<PriorFactor<Pose3> >(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);
// 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());

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

@ -116,8 +116,8 @@ TEST( SmartRangeFactor, optimization ) {
graph.push_back(f);
const noiseModel::Base::shared_ptr //
priorNoise = noiseModel::Diagonal::Sigmas(Vector3(1, 1, M_PI));
graph.push_back(PriorFactor<Pose2>(1, pose1, priorNoise));
graph.push_back(PriorFactor<Pose2>(2, pose2, priorNoise));
graph.emplace_shared<PriorFactor<Pose2> >(1, pose1, priorNoise);
graph.emplace_shared<PriorFactor<Pose2> >(2, pose2, priorNoise);
// Try optimizing
LevenbergMarquardtParams params;

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

@ -254,8 +254,8 @@ TEST( SmartStereoProjectionPoseFactor, 3poses_smart_projection_factor ) {
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.emplace_shared<PriorFactor<Pose3> >(x1, pose1, noisePrior);
graph.emplace_shared<PriorFactor<Pose3> >(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),
@ -394,8 +394,8 @@ TEST( SmartStereoProjectionPoseFactor, jacobianSVD ) {
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.emplace_shared<PriorFactor<Pose3> >(x1, pose1, noisePrior);
graph.emplace_shared<PriorFactor<Pose3> >(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),
@ -463,8 +463,8 @@ TEST( SmartStereoProjectionPoseFactor, landmarkDistance ) {
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.emplace_shared<PriorFactor<Pose3> >(x1, pose1, noisePrior);
graph.emplace_shared<PriorFactor<Pose3> >(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),
@ -545,8 +545,8 @@ TEST( SmartStereoProjectionPoseFactor, dynamicOutlierRejection ) {
graph.push_back(smartFactor2);
graph.push_back(smartFactor3);
graph.push_back(smartFactor4);
graph.push_back(PriorFactor<Pose3>(x1, pose1, noisePrior));
graph.push_back(PriorFactor<Pose3>(x2, pose2, noisePrior));
graph.emplace_shared<PriorFactor<Pose3> >(x1, pose1, noisePrior);
graph.emplace_shared<PriorFactor<Pose3> >(x2, pose2, noisePrior);
Pose3 noise_pose = Pose3(Rot3::Ypr(-M_PI / 100, 0., -M_PI / 100),
Point3(0.1, 0.1, 0.1)); // smaller noise

2
tests/testGeneralSFMFactorB.cpp
View File

@ -51,7 +51,7 @@ TEST(PinholeCamera, BAL) {
for (size_t j = 0; j < db.number_tracks(); j++) {
for (const SfM_Measurement& m: db.tracks[j].measurements)
graph.push_back(sfmFactor(m.second, unit2, m.first, P(j)));
graph.emplace_shared<sfmFactor>(m.second, unit2, m.first, P(j));
}
Values initial = initialCamerasAndPointsEstimate(db);

2
timing/timeSFMBAL.cpp
View File

@ -40,7 +40,7 @@ int main(int argc, char* argv[]) {
for (const SfM_Measurement& m: db.tracks[j].measurements) {
size_t i = m.first;
Point2 z = m.second;
graph.push_back(SfmFactor(z, gNoiseModel, C(i), P(j)));
graph.emplace_shared<SfmFactor>(z, gNoiseModel, C(i), P(j));
}
}