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

Merge branch 'develop' into feature/actions_ci_add 

# Conflicts:
#	.travis.yml
release/4.3a0
Frank Dellaert 2020-08-16 23:21:18 -04:00
parent 84049ebecf aceaa139ab
commit 42e1af5358
10 changed files with 901 additions and 575 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

1
cython/gtsam/utils/plot.py
View File

@ -155,7 +155,6 @@ def plot_pose2(fignum, pose, axis_length=0.1, covariance=None,
axes.set_xlabel(axis_labels[0])
axes.set_ylabel(axis_labels[1])
axes.set_zlabel(axis_labels[2])
return fig

32
examples/Data/example_with_vertices.g2o
View File

@ -1,16 +1,16 @@
VERTEX_SE3:QUAT 8646911284551352320 40 -1.15443e-13 10 0.557345 0.557345 -0.435162 -0.435162
VERTEX_SE3:QUAT 8646911284551352321 28.2843 28.2843 10 0.301633 0.728207 -0.568567 -0.235508
VERTEX_SE3:QUAT 8646911284551352322 -1.6986e-08 40 10 -3.89609e-10 0.788205 -0.615412 -2.07622e-10
VERTEX_SE3:QUAT 8646911284551352323 -28.2843 28.2843 10 -0.301633 0.728207 -0.568567 0.235508
VERTEX_SE3:QUAT 8646911284551352324 -40 -2.32554e-10 10 -0.557345 0.557345 -0.435162 0.435162
VERTEX_SE3:QUAT 8646911284551352325 -28.2843 -28.2843 10 -0.728207 0.301633 -0.235508 0.568567
VERTEX_SE3:QUAT 8646911284551352326 -2.53531e-09 -40 10 -0.788205 -1.25891e-11 -3.82742e-13 0.615412
VERTEX_SE3:QUAT 8646911284551352327 28.2843 -28.2843 10 -0.728207 -0.301633 0.235508 0.568567
VERTEX_TRACKXYZ 7782220156096217088 10 10 10
VERTEX_TRACKXYZ 7782220156096217089 -10 10 10
VERTEX_TRACKXYZ 7782220156096217090 -10 -10 10
VERTEX_TRACKXYZ 7782220156096217091 10 -10 10
VERTEX_TRACKXYZ 7782220156096217092 10 10 -10
VERTEX_TRACKXYZ 7782220156096217093 -10 10 -10
VERTEX_TRACKXYZ 7782220156096217094 -10 -10 -10
VERTEX_TRACKXYZ 7782220156096217095 10 -10 -10
VERTEX_SE3:QUAT 0 40 -1.15443e-13 10 0.557345 0.557345 -0.435162 -0.435162
VERTEX_SE3:QUAT 1 28.2843 28.2843 10 0.301633 0.728207 -0.568567 -0.235508
VERTEX_SE3:QUAT 2 -1.6986e-08 40 10 -3.89609e-10 0.788205 -0.615412 -2.07622e-10
VERTEX_SE3:QUAT 3 -28.2843 28.2843 10 -0.301633 0.728207 -0.568567 0.235508
VERTEX_SE3:QUAT 4 -40 -2.32554e-10 10 -0.557345 0.557345 -0.435162 0.435162
VERTEX_SE3:QUAT 5 -28.2843 -28.2843 10 -0.728207 0.301633 -0.235508 0.568567
VERTEX_SE3:QUAT 6 -2.53531e-09 -40 10 -0.788205 -1.25891e-11 -3.82742e-13 0.615412
VERTEX_SE3:QUAT 7 28.2843 -28.2843 10 -0.728207 -0.301633 0.235508 0.568567
VERTEX_TRACKXYZ 0 10 10 10
VERTEX_TRACKXYZ 1 -10 10 10
VERTEX_TRACKXYZ 2 -10 -10 10
VERTEX_TRACKXYZ 3 10 -10 10
VERTEX_TRACKXYZ 4 10 10 -10
VERTEX_TRACKXYZ 5 -10 10 -10
VERTEX_TRACKXYZ 6 -10 -10 -10
VERTEX_TRACKXYZ 7 10 -10 -10

2
gtsam/nonlinear/Values.h
View File

@ -397,7 +397,7 @@ namespace gtsam {
template<class ValueType>
size_t count() const {
size_t i = 0;
for (const auto& key_value : *this) {
for (const auto key_value : *this) {
if (dynamic_cast<const GenericValue<ValueType>*>(&key_value.value))
++i;
}

19
gtsam/sam/BearingRangeFactor.h
View File

@ -42,14 +42,21 @@ class BearingRangeFactor
public:
typedef boost::shared_ptr<This> shared_ptr;
/// default constructor
/// Default constructor
BearingRangeFactor() {}
/// primary constructor
BearingRangeFactor(Key key1, Key key2, const B& measuredBearing,
const R& measuredRange, const SharedNoiseModel& model)
: Base({key1, key2}, model, T(measuredBearing, measuredRange)) {
this->initialize(expression({key1, key2}));
/// Construct from BearingRange instance
BearingRangeFactor(Key key1, Key key2, const T &bearingRange,
const SharedNoiseModel &model)
: Base({{key1, key2}}, model, T(bearingRange)) {
this->initialize(expression({{key1, key2}}));
}
/// Construct from separate bearing and range
BearingRangeFactor(Key key1, Key key2, const B &measuredBearing,
const R &measuredRange, const SharedNoiseModel &model)
: Base({{key1, key2}}, model, T(measuredBearing, measuredRange)) {
this->initialize(expression({{key1, key2}}));
}
virtual ~BearingRangeFactor() {}

93
gtsam/sfm/BinaryMeasurement.h
View File

@ -15,78 +15,69 @@
* @file BinaryMeasurement.h
* @author Akshay Krishnan
* @date July 2020
* @brief Binary measurement represents a measurement between two keys in a graph.
* A binary measurement is similar to a BetweenFactor, except that it does not contain
* an error function. It is a measurement (along with a noise model) from one key to
* another. Note that the direction is important. A measurement from key1 to key2 is not
* the same as the same measurement from key2 to key1.
* @brief Binary measurement represents a measurement between two keys in a
* graph. A binary measurement is similar to a BetweenFactor, except that it
* does not contain an error function. It is a measurement (along with a noise
* model) from one key to another. Note that the direction is important. A
* measurement from key1 to key2 is not the same as the same measurement from
* key2 to key1.
*/
#include <ostream>
#include <gtsam/base/Testable.h>
#include <gtsam/base/Lie.h>
#include <gtsam/nonlinear/NonlinearFactor.h>
#include <gtsam/inference/Factor.h>
#include <gtsam/inference/Key.h>
#include <gtsam/linear/NoiseModel.h>
#include <iostream>
#include <vector>
namespace gtsam {
template<class VALUE>
class BinaryMeasurement {
// Check that VALUE type is testable
BOOST_CONCEPT_ASSERT((IsTestable<VALUE>));
public:
typedef VALUE T;
template <class T> class BinaryMeasurement : public Factor {
// Check that T type is testable
BOOST_CONCEPT_ASSERT((IsTestable<T>));
public:
// shorthand for a smart pointer to a measurement
typedef typename boost::shared_ptr<BinaryMeasurement> shared_ptr;
using shared_ptr = typename boost::shared_ptr<BinaryMeasurement>;
private:
Key key1_, key2_; /** Keys */
private:
T measured_; ///< The measurement
SharedNoiseModel noiseModel_; ///< Noise model
VALUE measured_; /** The measurement */
public:
BinaryMeasurement(Key key1, Key key2, const T &measured,
const SharedNoiseModel &model = nullptr)
: Factor(std::vector<Key>({key1, key2})), measured_(measured),
noiseModel_(model) {}
SharedNoiseModel noiseModel_; /** Noise model */
public:
/** Constructor */
BinaryMeasurement(Key key1, Key key2, const VALUE &measured,
const SharedNoiseModel &model = nullptr) :
key1_(key1), key2_(key2), measured_(measured), noiseModel_(model) {
}
Key key1() const { return key1_; }
Key key2() const { return key2_; }
/// @name Standard Interface
/// @{
Key key1() const { return keys_[0]; }
Key key2() const { return keys_[1]; }
const T &measured() const { return measured_; }
const SharedNoiseModel &noiseModel() const { return noiseModel_; }
/** implement functions needed for Testable */
/// @}
/// @name Testable
/// @{
/** print */
void print(const std::string &s, const KeyFormatter &keyFormatter = DefaultKeyFormatter) const {
std::cout << s << "BinaryMeasurement("
<< keyFormatter(this->key1()) << ","
void print(const std::string &s,
const KeyFormatter &keyFormatter = DefaultKeyFormatter) const {
std::cout << s << "BinaryMeasurement(" << keyFormatter(this->key1()) << ","
<< keyFormatter(this->key2()) << ")\n";
traits<T>::Print(measured_, " measured: ");
this->noiseModel_->print(" noise model: ");
}
/** equals */
bool equals(const BinaryMeasurement &expected, double tol = 1e-9) const {
const BinaryMeasurement<VALUE> *e = dynamic_cast<const BinaryMeasurement<VALUE> *> (&expected);
return e != nullptr && key1_ == e->key1_ &&
key2_ == e->key2_
&& traits<VALUE>::Equals(this->measured_, e->measured_, tol) &&
noiseModel_->equals(*expected.noiseModel());
const BinaryMeasurement<T> *e =
dynamic_cast<const BinaryMeasurement<T> *>(&expected);
return e != nullptr && Factor::equals(*e) &&
traits<T>::Equals(this->measured_, e->measured_, tol) &&
noiseModel_->equals(*expected.noiseModel());
}
/** return the measured value */
VALUE measured() const {
return measured_;
}
}; // \class BetweenMeasurement
}
/// @}
};
} // namespace gtsam

1116
gtsam/slam/dataset.cpp
View File

File diff suppressed because it is too large Load Diff

105
gtsam/slam/dataset.h
View File

@ -20,13 +20,12 @@
#pragma once
#include <gtsam/sfm/BinaryMeasurement.h>
#include <gtsam/slam/BetweenFactor.h>
#include <gtsam/geometry/Cal3Bundler.h>
#include <gtsam/geometry/PinholeCamera.h>
#include <gtsam/geometry/Point2.h>
#include <gtsam/geometry/Point3.h>
#include <gtsam/geometry/Pose2.h>
#include <gtsam/geometry/Pose3.h>
#include <gtsam/geometry/Rot3.h>
#include <gtsam/nonlinear/NonlinearFactorGraph.h>
#include <gtsam/nonlinear/Values.h>
#include <gtsam/linear/NoiseModel.h>
@ -74,20 +73,42 @@ enum KernelFunctionType {
KernelFunctionTypeNONE, KernelFunctionTypeHUBER, KernelFunctionTypeTUKEY
};
/// Return type for auxiliary functions
typedef std::pair<Key, Pose2> IndexedPose;
typedef std::pair<Key, Point2> IndexedLandmark;
typedef std::pair<std::pair<Key, Key>, Pose2> IndexedEdge;
#ifdef GTSAM_ALLOW_DEPRECATED_SINCE_V41
/**
* Parse TORO/G2O vertex "id x y yaw"
* @param is input stream
* @param tag string parsed from input stream, will only parse if vertex type
* Parse variables in a line-based text format (like g2o) into a map.
* Instantiated in .cpp Pose2, Point2, Pose3, and Point3.
* Note the map keys are integer indices, *not* gtsam::Keys. This is is
* different below where landmarks will use L(index) symbols.
*/
GTSAM_EXPORT boost::optional<IndexedPose> parseVertex(std::istream& is,
const std::string& tag);
#endif
template <typename T>
GTSAM_EXPORT std::map<size_t, T> parseVariables(const std::string &filename,
size_t maxIndex = 0);
/**
* Parse binary measurements in a line-based text format (like g2o) into a
* vector. Instantiated in .cpp for Pose2, Rot2, Pose3, and Rot3. The rotation
* versions parse poses and extract only the rotation part, using the marginal
* covariance as noise model.
*/
template <typename T>
GTSAM_EXPORT std::vector<BinaryMeasurement<T>>
parseMeasurements(const std::string &filename,
const noiseModel::Diagonal::shared_ptr &model = nullptr,
size_t maxIndex = 0);
/**
* Parse BetweenFactors in a line-based text format (like g2o) into a vector of
* shared pointers. Instantiated in .cpp T equal to Pose2 and Pose3.
*/
template <typename T>
GTSAM_EXPORT std::vector<typename BetweenFactor<T>::shared_ptr>
parseFactors(const std::string &filename,
const noiseModel::Diagonal::shared_ptr &model = nullptr,
size_t maxIndex = 0);
/// Return type for auxiliary functions
typedef std::pair<size_t, Pose2> IndexedPose;
typedef std::pair<size_t, Point2> IndexedLandmark;
typedef std::pair<std::pair<size_t, size_t>, Pose2> IndexedEdge;
/**
* Parse TORO/G2O vertex "id x y yaw"
@ -102,7 +123,6 @@ GTSAM_EXPORT boost::optional<IndexedPose> parseVertexPose(std::istream& is,
* @param is input stream
* @param tag string parsed from input stream, will only parse if vertex type
*/
GTSAM_EXPORT boost::optional<IndexedLandmark> parseVertexLandmark(std::istream& is,
const std::string& tag);
@ -114,18 +134,21 @@ GTSAM_EXPORT boost::optional<IndexedLandmark> parseVertexLandmark(std::istream&
GTSAM_EXPORT boost::optional<IndexedEdge> parseEdge(std::istream& is,
const std::string& tag);
/// Return type for load functions
typedef std::pair<NonlinearFactorGraph::shared_ptr, Values::shared_ptr> GraphAndValues;
/// Return type for load functions, which return a graph and initial values. For
/// landmarks, the gtsam::Symbol L(index) is used to insert into the Values.
/// Bearing-range measurements also refer to landmarks with L(index).
using GraphAndValues =
std::pair<NonlinearFactorGraph::shared_ptr, Values::shared_ptr>;
/**
* Load TORO 2D Graph
* @param dataset/model pair as constructed by [dataset]
* @param maxID if non-zero cut out vertices >= maxID
* @param maxIndex if non-zero cut out vertices >= maxIndex
* @param addNoise add noise to the edges
* @param smart try to reduce complexity of covariance to cheapest model
*/
GTSAM_EXPORT GraphAndValues load2D(
std::pair<std::string, SharedNoiseModel> dataset, int maxID = 0,
std::pair<std::string, SharedNoiseModel> dataset, size_t maxIndex = 0,
bool addNoise = false,
bool smart = true, //
NoiseFormat noiseFormat = NoiseFormatAUTO,
@ -135,7 +158,7 @@ GTSAM_EXPORT GraphAndValues load2D(
* Load TORO/G2O style graph files
* @param filename
* @param model optional noise model to use instead of one specified by file
* @param maxID if non-zero cut out vertices >= maxID
* @param maxIndex if non-zero cut out vertices >= maxIndex
* @param addNoise add noise to the edges
* @param smart try to reduce complexity of covariance to cheapest model
* @param noiseFormat how noise parameters are stored
@ -143,13 +166,13 @@ GTSAM_EXPORT GraphAndValues load2D(
* @return graph and initial values
*/
GTSAM_EXPORT GraphAndValues load2D(const std::string& filename,
SharedNoiseModel model = SharedNoiseModel(), Key maxID = 0, bool addNoise =
SharedNoiseModel model = SharedNoiseModel(), size_t maxIndex = 0, bool addNoise =
false, bool smart = true, NoiseFormat noiseFormat = NoiseFormatAUTO, //
KernelFunctionType kernelFunctionType = KernelFunctionTypeNONE);
/// @deprecated load2D now allows for arbitrary models and wrapping a robust kernel
GTSAM_EXPORT GraphAndValues load2D_robust(const std::string& filename,
noiseModel::Base::shared_ptr& model, int maxID = 0);
noiseModel::Base::shared_ptr& model, size_t maxIndex = 0);
/** save 2d graph */
GTSAM_EXPORT void save2D(const NonlinearFactorGraph& graph,
@ -173,21 +196,15 @@ GTSAM_EXPORT GraphAndValues readG2o(const std::string& g2oFile, const bool is3D
* @param filename The name of the g2o file to write
* @param graph NonlinearFactor graph storing the measurements
* @param estimate Values
*
* Note:behavior change in PR #471: to be consistent with load2D and load3D, we
* write the *indices* to file and not the full Keys. This change really only
* affects landmarks, which get read as indices but stored in values with the
* symbol L(index).
*/
GTSAM_EXPORT void writeG2o(const NonlinearFactorGraph& graph,
const Values& estimate, const std::string& filename);
/// 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,
const noiseModel::Diagonal::shared_ptr& corruptingNoise=nullptr);
/// Parse vertices in 3D TORO/g2o graph file into a map of Pose3s.
GTSAM_EXPORT std::map<Key, Pose3> parse3DPoses(const std::string& filename);
/// Parse landmarks in 3D g2o graph file into a map of Point3s.
GTSAM_EXPORT std::map<Key, Point3> parse3DLandmarks(const string& filename);
/// Load TORO 3D Graph
GTSAM_EXPORT GraphAndValues load3D(const std::string& filename);
@ -324,4 +341,24 @@ GTSAM_EXPORT Values initialCamerasEstimate(const SfmData& db);
*/
GTSAM_EXPORT Values initialCamerasAndPointsEstimate(const SfmData& db);
// To be deprecated when pybind wrapper is merged:
using BetweenFactorPose3s = std::vector<BetweenFactor<Pose3>::shared_ptr>;
GTSAM_EXPORT BetweenFactorPose3s
parse3DFactors(const std::string &filename,
const noiseModel::Diagonal::shared_ptr &model = nullptr,
size_t maxIndex = 0);
#ifdef GTSAM_ALLOW_DEPRECATED_SINCE_V41
inline boost::optional<IndexedPose> parseVertex(std::istream &is,
const std::string &tag) {
return parseVertexPose(is, tag);
}
GTSAM_EXPORT std::map<size_t, Pose3> parse3DPoses(const std::string &filename,
size_t maxIndex = 0);
GTSAM_EXPORT std::map<size_t, Point3>
parse3DLandmarks(const std::string &filename, size_t maxIndex = 0);
#endif
} // namespace gtsam

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

@ -82,38 +82,73 @@ TEST( dataSet, parseEdge)
const auto actual = parseEdge(is, tag);
EXPECT(actual);
if (actual) {
pair<Key, Key> expected(0, 1);
pair<size_t, size_t> expected(0, 1);
EXPECT(expected == actual->first);
EXPECT(assert_equal(Pose2(2, 3, 4), actual->second));
}
}
/* ************************************************************************* */
TEST( dataSet, load2D)
{
TEST(dataSet, load2D) {
///< The structure where we will save the SfM data
const string filename = findExampleDataFile("w100.graph");
NonlinearFactorGraph::shared_ptr graph;
Values::shared_ptr initial;
boost::tie(graph, initial) = load2D(filename);
EXPECT_LONGS_EQUAL(300,graph->size());
EXPECT_LONGS_EQUAL(100,initial->size());
noiseModel::Unit::shared_ptr model = noiseModel::Unit::Create(3);
BetweenFactor<Pose2> expected(1, 0, Pose2(-0.99879,0.0417574,-0.00818381), model);
BetweenFactor<Pose2>::shared_ptr actual = boost::dynamic_pointer_cast<
BetweenFactor<Pose2> >(graph->at(0));
EXPECT_LONGS_EQUAL(300, graph->size());
EXPECT_LONGS_EQUAL(100, initial->size());
auto model = noiseModel::Unit::Create(3);
BetweenFactor<Pose2> expected(1, 0, Pose2(-0.99879, 0.0417574, -0.00818381),
model);
BetweenFactor<Pose2>::shared_ptr actual =
boost::dynamic_pointer_cast<BetweenFactor<Pose2>>(graph->at(0));
EXPECT(assert_equal(expected, *actual));
// Check binary measurements, Pose2
size_t maxIndex = 5;
auto measurements = parseMeasurements<Pose2>(filename, nullptr, maxIndex);
EXPECT_LONGS_EQUAL(5, measurements.size());
// Check binary measurements, Rot2
auto measurements2 = parseMeasurements<Rot2>(filename);
EXPECT_LONGS_EQUAL(300, measurements2.size());
// // Check factor parsing
const auto actualFactors = parseFactors<Pose2>(filename);
for (size_t i : {0, 1, 2, 3, 4, 5}) {
EXPECT(assert_equal(
*boost::dynamic_pointer_cast<BetweenFactor<Pose2>>(graph->at(i)),
*actualFactors[i], 1e-5));
}
// Check pose parsing
const auto actualPoses = parseVariables<Pose2>(filename);
for (size_t j : {0, 1, 2, 3, 4}) {
EXPECT(assert_equal(initial->at<Pose2>(j), actualPoses.at(j), 1e-5));
}
// Check landmark parsing
const auto actualLandmarks = parseVariables<Point2>(filename);
EXPECT_LONGS_EQUAL(0, actualLandmarks.size());
}
/* ************************************************************************* */
TEST( dataSet, load2DVictoriaPark)
{
TEST(dataSet, load2DVictoriaPark) {
const string filename = findExampleDataFile("victoria_park.txt");
NonlinearFactorGraph::shared_ptr graph;
Values::shared_ptr initial;
// Load all
boost::tie(graph, initial) = load2D(filename);
EXPECT_LONGS_EQUAL(10608,graph->size());
EXPECT_LONGS_EQUAL(7120,initial->size());
EXPECT_LONGS_EQUAL(10608, graph->size());
EXPECT_LONGS_EQUAL(7120, initial->size());
// Restrict keys
size_t maxIndex = 5;
boost::tie(graph, initial) = load2D(filename, nullptr, maxIndex);
EXPECT_LONGS_EQUAL(5, graph->size());
EXPECT_LONGS_EQUAL(6, initial->size()); // file has 0 as well
EXPECT_LONGS_EQUAL(L(5), graph->at(4)->keys()[1]);
}
/* ************************************************************************* */
@ -184,7 +219,7 @@ TEST(dataSet, readG2o3D) {
}
// Check factor parsing
const auto actualFactors = parse3DFactors(g2oFile);
const auto actualFactors = parseFactors<Pose3>(g2oFile);
for (size_t i : {0, 1, 2, 3, 4, 5}) {
EXPECT(assert_equal(
*boost::dynamic_pointer_cast<BetweenFactor<Pose3>>(expectedGraph[i]),
@ -192,13 +227,13 @@ TEST(dataSet, readG2o3D) {
}
// Check pose parsing
const auto actualPoses = parse3DPoses(g2oFile);
const auto actualPoses = parseVariables<Pose3>(g2oFile);
for (size_t j : {0, 1, 2, 3, 4}) {
EXPECT(assert_equal(poses[j], actualPoses.at(j), 1e-5));
}
// Check landmark parsing
const auto actualLandmarks = parse3DLandmarks(g2oFile);
const auto actualLandmarks = parseVariables<Point3>(g2oFile);
for (size_t j : {0, 1, 2, 3, 4}) {
EXPECT(assert_equal(poses[j], actualPoses.at(j), 1e-5));
}
@ -259,7 +294,7 @@ TEST(dataSet, readG2oCheckDeterminants) {
const string g2oFile = findExampleDataFile("toyExample.g2o");
// Check determinants in factors
auto factors = parse3DFactors(g2oFile);
auto factors = parseFactors<Pose3>(g2oFile);
EXPECT_LONGS_EQUAL(6, factors.size());
for (const auto& factor : factors) {
const Rot3 R = factor->measured().rotation();
@ -267,13 +302,13 @@ TEST(dataSet, readG2oCheckDeterminants) {
}
// Check determinants in initial values
const map<Key, Pose3> poses = parse3DPoses(g2oFile);
const map<size_t, Pose3> poses = parseVariables<Pose3>(g2oFile);
EXPECT_LONGS_EQUAL(5, poses.size());
for (const auto& key_value : poses) {
const Rot3 R = key_value.second.rotation();
EXPECT_DOUBLES_EQUAL(1.0, R.matrix().determinant(), 1e-9);
}
const map<Key, Point3> landmarks = parse3DLandmarks(g2oFile);
const map<size_t, Point3> landmarks = parseVariables<Point3>(g2oFile);
EXPECT_LONGS_EQUAL(0, landmarks.size());
}
@ -282,10 +317,13 @@ TEST(dataSet, readG2oLandmarks) {
const string g2oFile = findExampleDataFile("example_with_vertices.g2o");
// Check number of poses and landmarks. Should be 8 each.
const map<Key, Pose3> poses = parse3DPoses(g2oFile);
const map<size_t, Pose3> poses = parseVariables<Pose3>(g2oFile);
EXPECT_LONGS_EQUAL(8, poses.size());
const map<Key, Point3> landmarks = parse3DLandmarks(g2oFile);
const map<size_t, Point3> landmarks = parseVariables<Point3>(g2oFile);
EXPECT_LONGS_EQUAL(8, landmarks.size());
auto graphAndValues = load3D(g2oFile);
EXPECT(graphAndValues.second->exists(L(0)));
}
/* ************************************************************************* */
@ -538,14 +576,12 @@ TEST( dataSet, writeBALfromValues_Dubrovnik){
Values value;
for(size_t i=0; i < readData.number_cameras(); i++){ // for each camera
Key poseKey = symbol('x',i);
Pose3 pose = poseChange.compose(readData.cameras[i].pose());
value.insert(poseKey, pose);
value.insert(X(i), pose);
}
for(size_t j=0; j < readData.number_tracks(); j++){ // for each point
Key pointKey = P(j);
Point3 point = poseChange.transformFrom( readData.tracks[j].p );
value.insert(pointKey, point);
value.insert(P(j), point);
}
// Write values and readData to a file
@ -570,13 +606,11 @@ TEST( dataSet, writeBALfromValues_Dubrovnik){
EXPECT(assert_equal(expected,actual,12));
Pose3 expectedPose = camera0.pose();
Key poseKey = symbol('x',0);
Pose3 actualPose = value.at<Pose3>(poseKey);
Pose3 actualPose = value.at<Pose3>(X(0));
EXPECT(assert_equal(expectedPose,actualPose, 1e-7));
Point3 expectedPoint = track0.p;
Key pointKey = P(0);
Point3 actualPoint = value.at<Point3>(pointKey);
Point3 actualPoint = value.at<Point3>(P(0));
EXPECT(assert_equal(expectedPoint,actualPoint, 1e-6));
}

2
tests/testNonlinearISAM.cpp
View File

@ -308,7 +308,7 @@ TEST(testNonlinearISAM, loop_closures ) {
// check if edge
const auto betweenPose = parseEdge(is, tag);
if (betweenPose) {
Key id1, id2;
size_t id1, id2;
tie(id1, id2) = betweenPose->first;
graph.emplace_shared<BetweenFactor<Pose2> >(Symbol('x', id2),
Symbol('x', id1), betweenPose->second, model);

14
timing/timeFrobeniusFactor.cpp
View File

@ -57,8 +57,8 @@ int main(int argc, char* argv[]) {
}
// Read G2O file
const auto factors = parse3DFactors(g2oFile);
const auto poses = parse3DPoses(g2oFile);
const auto measurements = parseMeasurements<Rot3>(g2oFile);
const auto poses = parseVariables<Pose3>(g2oFile);
// Build graph
NonlinearFactorGraph graph;
@ -66,12 +66,12 @@ int main(int argc, char* argv[]) {
auto priorModel = noiseModel::Isotropic::Sigma(6, 10000);
graph.add(PriorFactor<SOn>(0, SOn::identity(4), priorModel));
auto G = boost::make_shared<Matrix>(SOn::VectorizedGenerators(4));
for (const auto& factor : factors) {
const auto& keys = factor->keys();
const auto& Tij = factor->measured();
const auto& model = factor->noiseModel();
for (const auto &m : measurements) {
const auto &keys = m.keys();
const Rot3 &Rij = m.measured();
const auto &model = m.noiseModel();
graph.emplace_shared<FrobeniusWormholeFactor>(
keys[0], keys[1], Rot3(Tij.rotation().matrix()), 4, model, G);
keys[0], keys[1], Rij, 4, model, G);
}
std::mt19937 rng(42);