Merge remote-tracking branch 'origin/develop' into feature/optimization_params
Frank Dellaert
commit
90f5dae3d0
|
|
@ -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()
|
||||
10
gtsam.h
10
gtsam.h
|
|
@ -2498,6 +2498,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,
|
||||
|
|
|
|||
|
|
@ -409,17 +409,17 @@ void save2D(const NonlinearFactorGraph& graph, const Values& config,
|
|||
|
||||
/* ************************************************************************* */
|
||||
GraphAndValues readG2o(const string& g2oFile, const bool is3D,
|
||||
KernelFunctionType kernelFunctionType) {
|
||||
// just call load2D
|
||||
int maxID = 0;
|
||||
bool addNoise = false;
|
||||
bool smart = true;
|
||||
|
||||
if(is3D)
|
||||
KernelFunctionType kernelFunctionType) {
|
||||
if (is3D) {
|
||||
return load3D(g2oFile);
|
||||
|
||||
return load2D(g2oFile, SharedNoiseModel(), maxID, addNoise, smart,
|
||||
NoiseFormatG2O, kernelFunctionType);
|
||||
} else {
|
||||
// just call load2D
|
||||
int maxID = 0;
|
||||
bool addNoise = false;
|
||||
bool smart = true;
|
||||
return load2D(g2oFile, SharedNoiseModel(), maxID, addNoise, smart,
|
||||
NoiseFormatG2O, kernelFunctionType);
|
||||
}
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
|
|
@ -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,44 +556,55 @@ 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
|
||||
mgtsam.block<3,3>(0,3) = m.block<3,3>(0,3); // off diagonal
|
||||
mgtsam.block<3,3>(3,0) = m.block<3,3>(3,0); // off diagonal
|
||||
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
|
||||
mgtsam.block<3, 3>(0, 3) = m.block<3, 3>(0, 3); // off diagonal
|
||||
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));
|
||||
graph->push_back(factor);
|
||||
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);
|
||||
}
|
||||
|
||||
|
|
|
|||
|
|
@ -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
|
||||
|
|
|
|||
|
|
@ -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));
|
||||
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));
|
||||
}
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
|
|
|
|||
Loading…
Reference in New Issue