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

Merge remote-tracking branch 'origin/feature/optimization_params' into feature/more_noise_wrapped

release/4.3a0
Frank Dellaert 2019-03-19 11:05:54 -04:00
parent a7e60a08fe 5af2256277
commit 5da004a732
8 changed files with 301 additions and 147 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

71
cython/gtsam/tests/test_NonlinearOptimizer.py Normal file
View File

@ -0,0 +1,71 @@
"""
GTSAM Copyright 2010-2019, Georgia Tech Research Corporation,
Atlanta, Georgia 30332-0415
All Rights Reserved
See LICENSE for the license information
Unit tests for IMU testing scenarios.
Author: Frank Dellaert & Duy Nguyen Ta (Python)
"""
# pylint: disable=invalid-name, no-name-in-module
from __future__ import print_function
import unittest
import gtsam
from gtsam import (DoglegOptimizer, DoglegParams, GaussNewtonOptimizer,
GaussNewtonParams, LevenbergMarquardtOptimizer,
LevenbergMarquardtParams, NonlinearFactorGraph, Ordering,
Point2, PriorFactorPoint2, Values)
KEY1 = 1
KEY2 = 2
class TestScenario(unittest.TestCase):
def test_optimize(self):
"""Do trivial test with three optimizer variants."""
fg = NonlinearFactorGraph()
model = gtsam.noiseModel_Unit.Create(2)
fg.add(PriorFactorPoint2(KEY1, Point2(0, 0), model))
# test error at minimum
xstar = Point2(0, 0)
optimal_values = Values()
optimal_values.insert(KEY1, xstar)
self.assertEqual(0.0, fg.error(optimal_values), 0.0)
# test error at initial = [(1-cos(3))^2 + (sin(3))^2]*50 =
x0 = Point2(3, 3)
initial_values = Values()
initial_values.insert(KEY1, x0)
self.assertEqual(9.0, fg.error(initial_values), 1e-3)
# optimize parameters
ordering = Ordering()
ordering.push_back(KEY1)
# Gauss-Newton
gnParams = GaussNewtonParams()
gnParams.setOrdering(ordering)
actual1 = GaussNewtonOptimizer(fg, initial_values, gnParams).optimize()
self.assertAlmostEqual(0, fg.error(actual1))
# Levenberg-Marquardt
lmParams = LevenbergMarquardtParams.CeresDefaults()
lmParams.setOrdering(ordering)
actual2 = LevenbergMarquardtOptimizer(
fg, initial_values, lmParams).optimize()
self.assertAlmostEqual(0, fg.error(actual2))
# Dogleg
dlParams = DoglegParams()
dlParams.setOrdering(ordering)
actual3 = DoglegOptimizer(fg, initial_values, dlParams).optimize()
self.assertAlmostEqual(0, fg.error(actual3))
if __name__ == "__main__":
unittest.main()

44
cython/gtsam/tests/test_dataset.py Normal file
View File

@ -0,0 +1,44 @@
"""
GTSAM Copyright 2010-2019, Georgia Tech Research Corporation,
Atlanta, Georgia 30332-0415
All Rights Reserved
See LICENSE for the license information
Unit tests for testing dataset access.
Author: Frank Dellaert
"""
# pylint: disable=invalid-name, no-name-in-module, no-member
from __future__ import print_function
import unittest
import gtsam
from gtsam import BetweenFactorPose3, BetweenFactorPose3s
class TestDataset(unittest.TestCase):
"""Tests for datasets.h wrapper."""
def setUp(self):
"""Get some common paths."""
self.pose3_example_g2o_file = gtsam.findExampleDataFile(
"pose3example.txt")
def test_readG2o3D(self):
"""Test reading directly into factor graph."""
is3D = True
graph, initial = gtsam.readG2o(self.pose3_example_g2o_file, is3D)
self.assertEqual(graph.size(), 6)
self.assertEqual(initial.size(), 5)
def test_parse3Dfactors(self):
"""Test parsing into data structure."""
factors = gtsam.parse3DFactors(self.pose3_example_g2o_file)
self.assertEqual(factors.size(), 6)
self.assertIsInstance(factors.at(0), BetweenFactorPose3)
if __name__ == '__main__':
unittest.main()

37
gtsam.h
View File

@ -2019,10 +2019,12 @@ virtual class NonlinearOptimizerParams {
void setVerbosity(string s);
string getLinearSolverType() const;
void setLinearSolverType(string solver);
void setOrdering(const gtsam::Ordering& ordering);
void setIterativeParams(gtsam::IterativeOptimizationParameters* params);
void setOrdering(const gtsam::Ordering& ordering);
string getOrderingType() const;
void setOrderingType(string ordering);
bool isMultifrontal() const;
bool isSequential() const;
@ -2043,15 +2045,32 @@ virtual class GaussNewtonParams : gtsam::NonlinearOptimizerParams {
virtual class LevenbergMarquardtParams : gtsam::NonlinearOptimizerParams {
LevenbergMarquardtParams();
double getlambdaInitial() const;
bool getDiagonalDamping() const;
double getlambdaFactor() const;
double getlambdaInitial() const;
double getlambdaLowerBound() const;
double getlambdaUpperBound() const;
bool getUseFixedLambdaFactor();
string getLogFile() const;
string getVerbosityLM() const;
void setlambdaInitial(double value);
void setDiagonalDamping(bool flag);
void setlambdaFactor(double value);
void setlambdaInitial(double value);
void setlambdaLowerBound(double value);
void setlambdaUpperBound(double value);
void setUseFixedLambdaFactor(bool flag);
void setLogFile(string s);
void setVerbosityLM(string s);
static gtsam::LevenbergMarquardtParams LegacyDefaults();
static gtsam::LevenbergMarquardtParams CeresDefaults();
static gtsam::LevenbergMarquardtParams EnsureHasOrdering(
gtsam::LevenbergMarquardtParams params,
const gtsam::NonlinearFactorGraph& graph);
static gtsam::LevenbergMarquardtParams ReplaceOrdering(
gtsam::LevenbergMarquardtParams params, const gtsam::Ordering& ordering);
};
#include <gtsam/nonlinear/DoglegOptimizer.h>
@ -2497,6 +2516,16 @@ void save2D(const gtsam::NonlinearFactorGraph& graph,
const gtsam::Values& config, gtsam::noiseModel::Diagonal* model,
string filename);
// std::vector<gtsam::BetweenFactor<Pose3>::shared_ptr>
class BetweenFactorPose3s
{
size_t size() const;
gtsam::BetweenFactorPose3* at(size_t i) const;
};
gtsam::BetweenFactorPose3s parse3DFactors(string filename);
pair<gtsam::NonlinearFactorGraph*, gtsam::Values*> load3D(string filename);
pair<gtsam::NonlinearFactorGraph*, gtsam::Values*> readG2o(string filename);
pair<gtsam::NonlinearFactorGraph*, gtsam::Values*> readG2o(string filename, bool is3D);
void writeG2o(const gtsam::NonlinearFactorGraph& graph,

15
gtsam/nonlinear/LevenbergMarquardtParams.h
View File

@ -122,24 +122,35 @@ public:
virtual ~LevenbergMarquardtParams() {}
void print(const std::string& str = "") const override;
/// @name Getters/Setters, mainly for MATLAB. Use fields above in C++.
/// @name Getters/Setters, mainly for wrappers. Use fields above in C++.
/// @{
bool getDiagonalDamping() const { return diagonalDamping; }
double getlambdaFactor() const { return lambdaFactor; }
double getlambdaInitial() const { return lambdaInitial; }
double getlambdaLowerBound() const { return lambdaLowerBound; }
double getlambdaUpperBound() const { return lambdaUpperBound; }
bool getUseFixedLambdaFactor() { return useFixedLambdaFactor; }
std::string getLogFile() const { return logFile; }
std::string getVerbosityLM() const { return verbosityLMTranslator(verbosityLM);}
void setDiagonalDamping(bool flag) { diagonalDamping = flag; }
void setlambdaFactor(double value) { lambdaFactor = value; }
void setlambdaInitial(double value) { lambdaInitial = value; }
void setlambdaLowerBound(double value) { lambdaLowerBound = value; }
void setlambdaUpperBound(double value) { lambdaUpperBound = value; }
void setLogFile(const std::string& s) { logFile = s; }
void setUseFixedLambdaFactor(bool flag) { useFixedLambdaFactor = flag;}
void setLogFile(const std::string& s) { logFile = s; }
void setVerbosityLM(const std::string& s) { verbosityLM = verbosityLMTranslator(s);}
// @}
/// @name Clone
/// @{
/// @return a deep copy of this object
boost::shared_ptr<NonlinearOptimizerParams> clone() const {
return boost::shared_ptr<NonlinearOptimizerParams>(new LevenbergMarquardtParams(*this));
}
/// @}
};
}

45
gtsam/nonlinear/NonlinearOptimizerParams.h
View File

@ -54,34 +54,16 @@ public:
}
virtual void print(const std::string& str = "") const;
size_t getMaxIterations() const {
return maxIterations;
}
double getRelativeErrorTol() const {
return relativeErrorTol;
}
double getAbsoluteErrorTol() const {
return absoluteErrorTol;
}
double getErrorTol() const {
return errorTol;
}
std::string getVerbosity() const {
return verbosityTranslator(verbosity);
}
size_t getMaxIterations() const { return maxIterations; }
double getRelativeErrorTol() const { return relativeErrorTol; }
double getAbsoluteErrorTol() const { return absoluteErrorTol; }
double getErrorTol() const { return errorTol; }
std::string getVerbosity() const { return verbosityTranslator(verbosity); }
void setMaxIterations(int value) {
maxIterations = value;
}
void setRelativeErrorTol(double value) {
relativeErrorTol = value;
}
void setAbsoluteErrorTol(double value) {
absoluteErrorTol = value;
}
void setErrorTol(double value) {
errorTol = value;
}
void setMaxIterations(int value) { maxIterations = value; }
void setRelativeErrorTol(double value) { relativeErrorTol = value; }
void setAbsoluteErrorTol(double value) { absoluteErrorTol = value; }
void setErrorTol(double value) { errorTol = value; }
void setVerbosity(const std::string& src) {
verbosity = verbosityTranslator(src);
}
@ -89,12 +71,7 @@ public:
static Verbosity verbosityTranslator(const std::string &s) ;
static std::string verbosityTranslator(Verbosity value) ;
// Successive Linearization Parameters
public:
/** See NonlinearOptimizerParams::linearSolverType */
enum LinearSolverType {
MULTIFRONTAL_CHOLESKY,
MULTIFRONTAL_QR,
@ -168,13 +145,9 @@ public:
private:
std::string linearSolverTranslator(LinearSolverType linearSolverType) const;
LinearSolverType linearSolverTranslator(const std::string& linearSolverType) const;
std::string orderingTypeTranslator(Ordering::OrderingType type) const;
Ordering::OrderingType orderingTypeTranslator(const std::string& type) const;
};
// For backward compatibility:

78
gtsam/slam/dataset.cpp
View File

@ -410,17 +410,17 @@ void save2D(const NonlinearFactorGraph& graph, const Values& config,
/* ************************************************************************* */
GraphAndValues readG2o(const string& g2oFile, const bool is3D,
KernelFunctionType kernelFunctionType) {
if (is3D) {
return load3D(g2oFile);
} else {
// just call load2D
int maxID = 0;
bool addNoise = false;
bool smart = true;
if(is3D)
return load3D(g2oFile);
return load2D(g2oFile, SharedNoiseModel(), maxID, addNoise, smart,
NoiseFormatG2O, kernelFunctionType);
}
}
/* ************************************************************************* */
void writeG2o(const NonlinearFactorGraph& graph, const Values& estimate,
@ -510,15 +510,12 @@ void writeG2o(const NonlinearFactorGraph& graph, const Values& estimate,
}
/* ************************************************************************* */
GraphAndValues load3D(const string& filename) {
std::map<Key, Pose3> parse3DPoses(const string& filename) {
ifstream is(filename.c_str());
if (!is)
throw invalid_argument("load3D: can not find file " + filename);
Values::shared_ptr initial(new Values);
NonlinearFactorGraph::shared_ptr graph(new NonlinearFactorGraph);
throw invalid_argument("parse3DPoses: can not find file " + filename);
std::map<Key, Pose3> poses;
while (!is.eof()) {
char buf[LINESIZE];
is.getline(buf, LINESIZE);
@ -530,22 +527,24 @@ GraphAndValues load3D(const string& filename) {
Key id;
double x, y, z, roll, pitch, yaw;
ls >> id >> x >> y >> z >> roll >> pitch >> yaw;
Rot3 R = Rot3::Ypr(yaw,pitch,roll);
Point3 t = Point3(x, y, z);
initial->insert(id, Pose3(R,t));
poses.emplace(id, Pose3(Rot3::Ypr(yaw, pitch, roll), {x, y, z}));
}
if (tag == "VERTEX_SE3:QUAT") {
Key id;
double x, y, z, qx, qy, qz, qw;
ls >> id >> x >> y >> z >> qx >> qy >> qz >> qw;
Rot3 R = Rot3::Quaternion(qw, qx, qy, qz);
Point3 t = Point3(x, y, z);
initial->insert(id, Pose3(R,t));
poses.emplace(id, Pose3(Rot3::Quaternion(qw, qx, qy, qz), {x, y, z}));
}
}
is.clear(); /* clears the end-of-file and error flags */
is.seekg(0, ios::beg);
return poses;
}
/* ************************************************************************* */
BetweenFactorPose3s parse3DFactors(const string& filename) {
ifstream is(filename.c_str());
if (!is) throw invalid_argument("parse3DFactors: can not find file " + filename);
std::vector<BetweenFactor<Pose3>::shared_ptr> factors;
while (!is.eof()) {
char buf[LINESIZE];
is.getline(buf, LINESIZE);
@ -557,33 +556,27 @@ GraphAndValues load3D(const string& filename) {
Key id1, id2;
double x, y, z, roll, pitch, yaw;
ls >> id1 >> id2 >> x >> y >> z >> roll >> pitch >> yaw;
Rot3 R = Rot3::Ypr(yaw,pitch,roll);
Point3 t = Point3(x, y, z);
Matrix m = I_6x6;
for (int i = 0; i < 6; i++)
for (int j = i; j < 6; j++)
ls >> m(i, j);
Matrix m(6, 6);
for (size_t i = 0; i < 6; i++)
for (size_t j = i; j < 6; j++) ls >> m(i, j);
SharedNoiseModel model = noiseModel::Gaussian::Information(m);
NonlinearFactor::shared_ptr factor(
new BetweenFactor<Pose3>(id1, id2, Pose3(R,t), model));
graph->push_back(factor);
factors.emplace_back(new BetweenFactor<Pose3>(
id1, id2, Pose3(Rot3::Ypr(yaw, pitch, roll), {x, y, z}), model));
}
if (tag == "EDGE_SE3:QUAT") {
Matrix m = I_6x6;
Key id1, id2;
double x, y, z, qx, qy, qz, qw;
ls >> id1 >> id2 >> x >> y >> z >> qx >> qy >> qz >> qw;
Rot3 R = Rot3::Quaternion(qw, qx, qy, qz);
Point3 t = Point3(x, y, z);
for (int i = 0; i < 6; i++){
for (int j = i; j < 6; j++){
Matrix m(6, 6);
for (size_t i = 0; i < 6; i++) {
for (size_t j = i; j < 6; j++) {
double mij;
ls >> mij;
m(i, j) = mij;
m(j, i) = mij;
}
}
Matrix mgtsam = I_6x6;
Matrix mgtsam(6, 6);
mgtsam.block<3, 3>(0, 0) = m.block<3, 3>(3, 3); // cov rotation
mgtsam.block<3, 3>(3, 3) = m.block<3, 3>(0, 0); // cov translation
@ -591,10 +584,27 @@ GraphAndValues load3D(const string& filename) {
mgtsam.block<3, 3>(3, 0) = m.block<3, 3>(3, 0); // off diagonal
SharedNoiseModel model = noiseModel::Gaussian::Information(mgtsam);
NonlinearFactor::shared_ptr factor(new BetweenFactor<Pose3>(id1, id2, Pose3(R,t), model));
factors.emplace_back(new BetweenFactor<Pose3>(
id1, id2, Pose3(Rot3::Quaternion(qw, qx, qy, qz), {x, y, z}), model));
}
}
return factors;
}
/* ************************************************************************* */
GraphAndValues load3D(const string& filename) {
const auto factors = parse3DFactors(filename);
NonlinearFactorGraph::shared_ptr graph(new NonlinearFactorGraph);
for (const auto& factor : factors) {
graph->push_back(factor);
}
const auto poses = parse3DPoses(filename);
Values::shared_ptr initial(new Values);
for (const auto& key_pose : poses) {
initial->insert(key_pose.first, key_pose.second);
}
return make_pair(graph, initial);
}

13
gtsam/slam/dataset.h
View File

@ -18,6 +18,7 @@
#pragma once
#include <gtsam/slam/BetweenFactor.h>
#include <gtsam/geometry/Cal3Bundler.h>
#include <gtsam/geometry/PinholeCamera.h>
#include <gtsam/geometry/Point2.h>
@ -34,6 +35,7 @@
#include <utility> // for pair
#include <vector>
#include <iosfwd>
#include <map>
namespace gtsam {
@ -153,9 +155,14 @@ GTSAM_EXPORT GraphAndValues readG2o(const std::string& g2oFile, const bool is3D
GTSAM_EXPORT void writeG2o(const NonlinearFactorGraph& graph,
const Values& estimate, const std::string& filename);
/**
* Load TORO 3D Graph
*/
/// Parse edges in 3D TORO graph file into a set of BetweenFactors.
using BetweenFactorPose3s = std::vector<gtsam::BetweenFactor<Pose3>::shared_ptr>;
GTSAM_EXPORT BetweenFactorPose3s parse3DFactors(const std::string& filename);
/// Parse vertices in 3D TORO graph file into a map of Pose3s.
GTSAM_EXPORT std::map<Key, Pose3> parse3DPoses(const std::string& filename);
/// Load TORO 3D Graph
GTSAM_EXPORT GraphAndValues load3D(const std::string& filename);
/// A measurement with its camera index

117
gtsam/slam/tests/testDataset.cpp
View File

@ -16,8 +16,8 @@
*/
#include <gtsam/slam/BetweenFactor.h>
#include <gtsam/slam/dataset.h>
#include <gtsam/slam/BetweenFactor.h>
#include <gtsam/geometry/Pose2.h>
#include <gtsam/inference/Symbol.h>
#include <gtsam/base/TestableAssertions.h>
@ -162,65 +162,74 @@ TEST( dataSet, readG2o)
}
/* ************************************************************************* */
TEST( dataSet, readG2o3D)
{
TEST(dataSet, readG2o3D) {
const string g2oFile = findExampleDataFile("pose3example");
auto model = noiseModel::Isotropic::Precision(6, 10000);
// Initialize 6 relative measurements with quaternion/point3 values:
const std::vector<Pose3> relative_poses = {
{{0.854230, 0.190253, 0.283162, -0.392318},
{1.001367, 0.015390, 0.004948}},
{{0.105373, 0.311512, 0.656877, -0.678505},
{0.523923, 0.776654, 0.326659}},
{{0.568551, 0.595795, -0.561677, 0.079353},
{0.910927, 0.055169, -0.411761}},
{{0.542221, -0.592077, 0.303380, -0.513226},
{0.775288, 0.228798, -0.596923}},
{{0.327419, -0.125250, -0.534379, 0.769122},
{-0.577841, 0.628016, -0.543592}},
{{0.083672, 0.104639, 0.627755, 0.766795},
{-0.623267, 0.086928, 0.773222}},
};
// Initialize 5 poses with quaternion/point3 values:
const std::vector<Pose3> poses = {
{{1.000000, 0.000000, 0.000000, 0.000000}, {0, 0, 0}},
{{0.854230, 0.190253, 0.283162, -0.392318},
{1.001367, 0.015390, 0.004948}},
{{0.421446, -0.351729, -0.597838, 0.584174},
{1.993500, 0.023275, 0.003793}},
{{0.067024, 0.331798, -0.200659, 0.919323},
{2.004291, 1.024305, 0.018047}},
{{0.765488, -0.035697, -0.462490, 0.445933},
{0.999908, 1.055073, 0.020212}},
};
// Specify connectivity:
using KeyPair = pair<Key, Key>;
std::vector<KeyPair> edges = {{0, 1}, {1, 2}, {2, 3}, {3, 4}, {1, 4}, {3, 0}};
// Created expected factor graph
size_t i = 0;
NonlinearFactorGraph expectedGraph;
for (const auto& keys : edges) {
expectedGraph.emplace_shared<BetweenFactor<Pose3>>(
keys.first, keys.second, relative_poses[i++], model);
}
// Check factor parsing
const auto actualFactors = parse3DFactors(g2oFile);
for (size_t i : {0, 1, 2, 3, 4, 5}) {
EXPECT(assert_equal(
*boost::dynamic_pointer_cast<BetweenFactor<Pose3>>(expectedGraph[i]),
*actualFactors[i], 1e-5));
}
// Check pose parsing
const auto actualPoses = parse3DPoses(g2oFile);
for (size_t j : {0, 1, 2, 3, 4}) {
EXPECT(assert_equal(poses[j], actualPoses.at(j), 1e-5));
}
// Check graph version
NonlinearFactorGraph::shared_ptr actualGraph;
Values::shared_ptr actualValues;
bool is3D = true;
boost::tie(actualGraph, actualValues) = readG2o(g2oFile, is3D);
Values expectedValues;
Rot3 R0 = Rot3::Quaternion(1.000000, 0.000000, 0.000000, 0.000000 );
Point3 p0 = Point3(0.000000, 0.000000, 0.000000);
expectedValues.insert(0, Pose3(R0, p0));
Rot3 R1 = Rot3::Quaternion(0.854230, 0.190253, 0.283162, -0.392318 );
Point3 p1 = Point3(1.001367, 0.015390, 0.004948);
expectedValues.insert(1, Pose3(R1, p1));
Rot3 R2 = Rot3::Quaternion(0.421446, -0.351729, -0.597838, 0.584174 );
Point3 p2 = Point3(1.993500, 0.023275, 0.003793);
expectedValues.insert(2, Pose3(R2, p2));
Rot3 R3 = Rot3::Quaternion(0.067024, 0.331798, -0.200659, 0.919323);
Point3 p3 = Point3(2.004291, 1.024305, 0.018047);
expectedValues.insert(3, Pose3(R3, p3));
Rot3 R4 = Rot3::Quaternion(0.765488, -0.035697, -0.462490, 0.445933);
Point3 p4 = Point3(0.999908, 1.055073, 0.020212);
expectedValues.insert(4, Pose3(R4, p4));
EXPECT(assert_equal(expectedValues,*actualValues,1e-5));
noiseModel::Diagonal::shared_ptr model = noiseModel::Diagonal::Precisions((Vector(6) << 10000.0,10000.0,10000.0,10000.0,10000.0,10000.0).finished());
NonlinearFactorGraph expectedGraph;
Point3 p01 = Point3(1.001367, 0.015390, 0.004948);
Rot3 R01 = Rot3::Quaternion(0.854230, 0.190253, 0.283162, -0.392318 );
expectedGraph.emplace_shared<BetweenFactor<Pose3> >(0, 1, Pose3(R01,p01), model);
Point3 p12 = Point3(0.523923, 0.776654, 0.326659);
Rot3 R12 = Rot3::Quaternion(0.105373 , 0.311512, 0.656877, -0.678505 );
expectedGraph.emplace_shared<BetweenFactor<Pose3> >(1, 2, Pose3(R12,p12), model);
Point3 p23 = Point3(0.910927, 0.055169, -0.411761);
Rot3 R23 = Rot3::Quaternion(0.568551 , 0.595795, -0.561677, 0.079353 );
expectedGraph.emplace_shared<BetweenFactor<Pose3> >(2, 3, Pose3(R23,p23), model);
Point3 p34 = Point3(0.775288, 0.228798, -0.596923);
Rot3 R34 = Rot3::Quaternion(0.542221 , -0.592077, 0.303380, -0.513226 );
expectedGraph.emplace_shared<BetweenFactor<Pose3> >(3, 4, Pose3(R34,p34), model);
Point3 p14 = Point3(-0.577841, 0.628016, -0.543592);
Rot3 R14 = Rot3::Quaternion(0.327419 , -0.125250, -0.534379, 0.769122 );
expectedGraph.emplace_shared<BetweenFactor<Pose3> >(1, 4, Pose3(R14,p14), model);
Point3 p30 = Point3(-0.623267, 0.086928, 0.773222);
Rot3 R30 = Rot3::Quaternion(0.083672 , 0.104639, 0.627755, 0.766795 );
expectedGraph.emplace_shared<BetweenFactor<Pose3> >(3, 0, Pose3(R30,p30), model);
EXPECT(assert_equal(expectedGraph, *actualGraph, 1e-5));
for (size_t j : {0, 1, 2, 3, 4}) {
EXPECT(assert_equal(poses[j], actualValues->at<Pose3>(j), 1e-5));
}
}
/* ************************************************************************* */