Wrapped parse3DFactors

release/4.3a0
Frank Dellaert 2019-03-14 01:25:06 -04:00
parent a47c52cb5e
commit 88ac6de4af
5 changed files with 33 additions and 9 deletions

View File

@ -8,26 +8,37 @@ See LICENSE for the license information
Unit tests for testing dataset access.
Author: Frank Dellaert
"""
# pylint: disable=invalid-name, E1101
# 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):
def setUp(self):
pass
"""Tests for datasets.h wrapper."""
def test_3d_graph(self):
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
g2o_file = gtsam.findExampleDataFile("pose3example.txt")
graph, initial = gtsam.readG2o(g2o_file, is3D)
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()

10
gtsam.h
View File

@ -2480,6 +2480,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,

View File

@ -540,7 +540,7 @@ std::map<Key, Pose3> parse3DPoses(const string& filename) {
}
/* ************************************************************************* */
std::vector<BetweenFactor<Pose3>::shared_ptr> parse3DFactors(const string& filename) {
BetweenFactorPose3s parse3DFactors(const string& filename) {
ifstream is(filename.c_str());
if (!is) throw invalid_argument("parse3DFactors: can not find file " + filename);

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 {
@ -154,7 +156,8 @@ 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.
GTSAM_EXPORT std::vector<BetweenFactor<Pose3>::shared_ptr> parse3DFactors(const std::string& filename);
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);

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>