Merge branch '3DInitialization-2'
Luca
commit
0c7368f408
|
|
@ -0,0 +1,11 @@
|
||||||
|
VERTEX_SE3:QUAT 0 0.000000 0.000000 0.000000 0.0008187 0.0011723 0.0895466 0.9959816
|
||||||
|
VERTEX_SE3:QUAT 1 0.000000 -0.000000 0.000000 0.0010673 0.0015636 0.1606931 0.9870026
|
||||||
|
VERTEX_SE3:QUAT 2 -0.388822 0.632954 0.001223 0.0029920 0.0014066 0.0258235 0.9996610
|
||||||
|
VERTEX_SE3:QUAT 3 -1.143204 0.050638 0.006026 -0.0012800 -0.0002767 -0.2850291 0.9585180
|
||||||
|
VERTEX_SE3:QUAT 4 -0.512416 0.486441 0.005171 0.0002681 0.0023574 0.0171476 0.9998502
|
||||||
|
EDGE_SE3:QUAT 1 2 1.000000 2.000000 0.000000 0.0000000 0.0000000 0.7071068 0.7071068 100.000000 0.000000 0.000000 0.000000 0.000000 0.000000 100.000000 0.000000 0.000000 0.000000 0.000000 100.000000 0.000000 0.000000 0.000000 100.000000 0.000000 0.000000 100.000000 0.000000 100.000000
|
||||||
|
EDGE_SE3:QUAT 2 3 -0.000000 1.000000 0.000000 0.0000000 0.0000000 0.7071068 0.7071068 100.000000 0.000000 0.000000 0.000000 0.000000 0.000000 100.000000 0.000000 0.000000 0.000000 0.000000 100.000000 0.000000 0.000000 0.000000 100.000000 0.000000 0.000000 100.000000 0.000000 100.000000
|
||||||
|
EDGE_SE3:QUAT 3 4 1.000000 1.000000 0.000000 0.0000000 0.0000000 0.7071068 0.7071068 100.000000 0.000000 0.000000 0.000000 0.000000 0.000000 100.000000 0.000000 0.000000 0.000000 0.000000 100.000000 0.000000 0.000000 0.000000 100.000000 0.000000 0.000000 100.000000 0.000000 100.000000
|
||||||
|
EDGE_SE3:QUAT 3 1 0.000001 2.000000 0.000000 0.0000000 0.0000000 1.0000000 0.0000002 100.000000 0.000000 0.000000 0.000000 0.000000 0.000000 100.000000 0.000000 0.000000 0.000000 0.000000 100.000000 0.000000 0.000000 0.000000 100.000000 0.000000 0.000000 100.000000 0.000000 100.000000
|
||||||
|
EDGE_SE3:QUAT 1 4 -1.000000 1.000000 0.000000 0.0000000 0.0000000 -0.7071068 0.7071068 100.000000 0.000000 0.000000 0.000000 0.000000 0.000000 100.000000 0.000000 0.000000 0.000000 0.000000 100.000000 0.000000 0.000000 0.000000 100.000000 0.000000 0.000000 100.000000 0.000000 100.000000
|
||||||
|
EDGE_SE3:QUAT 0 1 0.000000 0.000000 0.000000 0.0000000 0.0000000 0.0000000 1.0000000 100.000000 0.000000 0.000000 0.000000 0.000000 0.000000 100.000000 0.000000 0.000000 0.000000 0.000000 100.000000 0.000000 0.000000 0.000000 100.000000 0.000000 0.000000 100.000000 0.000000 100.000000
|
||||||
|
|
@ -45,11 +45,21 @@ int main(const int argc, const char *argv[]) {
|
||||||
noiseModel::Diagonal::Variances((Vector(3) << 1e-6, 1e-6, 1e-8));
|
noiseModel::Diagonal::Variances((Vector(3) << 1e-6, 1e-6, 1e-8));
|
||||||
graphWithPrior.add(PriorFactor<Pose2>(0, Pose2(), priorModel));
|
graphWithPrior.add(PriorFactor<Pose2>(0, Pose2(), priorModel));
|
||||||
|
|
||||||
|
GaussNewtonParams params;
|
||||||
|
params.setVerbosity("TERMINATION");
|
||||||
|
if (argc == 4) {
|
||||||
|
params.maxIterations = atoi(argv[3]);
|
||||||
|
std::cout << "User required to perform " << params.maxIterations << " iterations "<< std::endl;
|
||||||
|
}
|
||||||
|
|
||||||
std::cout << "Optimizing the factor graph" << std::endl;
|
std::cout << "Optimizing the factor graph" << std::endl;
|
||||||
GaussNewtonOptimizer optimizer(graphWithPrior, *initial);
|
GaussNewtonOptimizer optimizer(graphWithPrior, *initial, params);
|
||||||
Values result = optimizer.optimize();
|
Values result = optimizer.optimize();
|
||||||
std::cout << "Optimization complete" << std::endl;
|
std::cout << "Optimization complete" << std::endl;
|
||||||
|
|
||||||
|
std::cout << "initial error=" <<graph->error(*initial)<< std::endl;
|
||||||
|
std::cout << "final error=" <<graph->error(result)<< std::endl;
|
||||||
|
|
||||||
if (argc < 3) {
|
if (argc < 3) {
|
||||||
result.print("result");
|
result.print("result");
|
||||||
} else {
|
} else {
|
||||||
|
|
|
||||||
|
|
@ -0,0 +1,89 @@
|
||||||
|
/* ----------------------------------------------------------------------------
|
||||||
|
|
||||||
|
* GTSAM Copyright 2010, Georgia Tech Research Corporation,
|
||||||
|
* Atlanta, Georgia 30332-0415
|
||||||
|
* All Rights Reserved
|
||||||
|
* Authors: Frank Dellaert, et al. (see THANKS for the full author list)
|
||||||
|
|
||||||
|
* See LICENSE for the license information
|
||||||
|
|
||||||
|
* -------------------------------------------------------------------------- */
|
||||||
|
|
||||||
|
/**
|
||||||
|
* @file Pose3SLAMExample_initializePose3.cpp
|
||||||
|
* @brief A 3D Pose SLAM example that reads input from g2o, and initializes the Pose3 using InitializePose3
|
||||||
|
* Syntax for the script is ./Pose3SLAMExample_changeKeys input.g2o rewritted.g2o
|
||||||
|
* @date Aug 25, 2014
|
||||||
|
* @author Luca Carlone
|
||||||
|
*/
|
||||||
|
|
||||||
|
#include <gtsam/slam/dataset.h>
|
||||||
|
#include <gtsam/slam/BetweenFactor.h>
|
||||||
|
#include <gtsam/slam/PriorFactor.h>
|
||||||
|
#include <fstream>
|
||||||
|
|
||||||
|
using namespace std;
|
||||||
|
using namespace gtsam;
|
||||||
|
|
||||||
|
int main(const int argc, const char *argv[]) {
|
||||||
|
|
||||||
|
// Read graph from file
|
||||||
|
string g2oFile;
|
||||||
|
if (argc < 2)
|
||||||
|
g2oFile = findExampleDataFile("pose3example.txt");
|
||||||
|
else
|
||||||
|
g2oFile = argv[1];
|
||||||
|
|
||||||
|
NonlinearFactorGraph::shared_ptr graph;
|
||||||
|
Values::shared_ptr initial;
|
||||||
|
bool is3D = true;
|
||||||
|
boost::tie(graph, initial) = readG2o(g2oFile, is3D);
|
||||||
|
|
||||||
|
bool add = false;
|
||||||
|
Key firstKey = 8646911284551352320;
|
||||||
|
|
||||||
|
std::cout << "Using reference key: " << firstKey << std::endl;
|
||||||
|
if(add)
|
||||||
|
std::cout << "adding key " << std::endl;
|
||||||
|
else
|
||||||
|
std::cout << "subtracting key " << std::endl;
|
||||||
|
|
||||||
|
|
||||||
|
if (argc < 3) {
|
||||||
|
std::cout << "Please provide output file to write " << std::endl;
|
||||||
|
} else {
|
||||||
|
const string inputFileRewritten = argv[2];
|
||||||
|
std::cout << "Rewriting input to file: " << inputFileRewritten << std::endl;
|
||||||
|
// Additional: rewrite input with simplified keys 0,1,...
|
||||||
|
Values simpleInitial;
|
||||||
|
BOOST_FOREACH(const Values::ConstKeyValuePair& key_value, *initial) {
|
||||||
|
Key key;
|
||||||
|
if(add)
|
||||||
|
key = key_value.key + firstKey;
|
||||||
|
else
|
||||||
|
key = key_value.key - firstKey;
|
||||||
|
|
||||||
|
simpleInitial.insert(key, initial->at(key_value.key));
|
||||||
|
}
|
||||||
|
NonlinearFactorGraph simpleGraph;
|
||||||
|
BOOST_FOREACH(const boost::shared_ptr<NonlinearFactor>& factor, *graph) {
|
||||||
|
boost::shared_ptr<BetweenFactor<Pose3> > pose3Between =
|
||||||
|
boost::dynamic_pointer_cast<BetweenFactor<Pose3> >(factor);
|
||||||
|
if (pose3Between){
|
||||||
|
Key key1, key2;
|
||||||
|
if(add){
|
||||||
|
key1 = pose3Between->key1() + firstKey;
|
||||||
|
key2 = pose3Between->key2() + firstKey;
|
||||||
|
}else{
|
||||||
|
key1 = pose3Between->key1() - firstKey;
|
||||||
|
key2 = pose3Between->key2() - firstKey;
|
||||||
|
}
|
||||||
|
NonlinearFactor::shared_ptr simpleFactor(
|
||||||
|
new BetweenFactor<Pose3>(key1, key2, pose3Between->measured(), pose3Between->get_noiseModel()));
|
||||||
|
simpleGraph.add(simpleFactor);
|
||||||
|
}
|
||||||
|
}
|
||||||
|
writeG2o(simpleGraph, simpleInitial, inputFileRewritten);
|
||||||
|
}
|
||||||
|
return 0;
|
||||||
|
}
|
||||||
|
|
@ -0,0 +1,74 @@
|
||||||
|
/* ----------------------------------------------------------------------------
|
||||||
|
|
||||||
|
* GTSAM Copyright 2010, Georgia Tech Research Corporation,
|
||||||
|
* Atlanta, Georgia 30332-0415
|
||||||
|
* All Rights Reserved
|
||||||
|
* Authors: Frank Dellaert, et al. (see THANKS for the full author list)
|
||||||
|
|
||||||
|
* See LICENSE for the license information
|
||||||
|
|
||||||
|
* -------------------------------------------------------------------------- */
|
||||||
|
|
||||||
|
/**
|
||||||
|
* @file Pose3SLAMExample_initializePose3.cpp
|
||||||
|
* @brief A 3D Pose SLAM example that reads input from g2o, and initializes the Pose3 using InitializePose3
|
||||||
|
* Syntax for the script is ./Pose3SLAMExample_initializePose3 input.g2o output.g2o
|
||||||
|
* @date Aug 25, 2014
|
||||||
|
* @author Luca Carlone
|
||||||
|
*/
|
||||||
|
|
||||||
|
#include <gtsam/slam/dataset.h>
|
||||||
|
#include <gtsam/slam/BetweenFactor.h>
|
||||||
|
#include <gtsam/slam/PriorFactor.h>
|
||||||
|
#include <gtsam/nonlinear/GaussNewtonOptimizer.h>
|
||||||
|
#include <fstream>
|
||||||
|
|
||||||
|
using namespace std;
|
||||||
|
using namespace gtsam;
|
||||||
|
|
||||||
|
int main(const int argc, const char *argv[]) {
|
||||||
|
|
||||||
|
// Read graph from file
|
||||||
|
string g2oFile;
|
||||||
|
if (argc < 2)
|
||||||
|
g2oFile = findExampleDataFile("pose3example.txt");
|
||||||
|
else
|
||||||
|
g2oFile = argv[1];
|
||||||
|
|
||||||
|
NonlinearFactorGraph::shared_ptr graph;
|
||||||
|
Values::shared_ptr initial;
|
||||||
|
bool is3D = true;
|
||||||
|
boost::tie(graph, initial) = readG2o(g2oFile, is3D);
|
||||||
|
|
||||||
|
// Add prior on the first key
|
||||||
|
NonlinearFactorGraph graphWithPrior = *graph;
|
||||||
|
noiseModel::Diagonal::shared_ptr priorModel = //
|
||||||
|
noiseModel::Diagonal::Variances((Vector(6) << 1e-6, 1e-6, 1e-6, 1e-4, 1e-4, 1e-4));
|
||||||
|
Key firstKey = 0;
|
||||||
|
BOOST_FOREACH(const Values::ConstKeyValuePair& key_value, *initial) {
|
||||||
|
std::cout << "Adding prior to g2o file " << std::endl;
|
||||||
|
firstKey = key_value.key;
|
||||||
|
graphWithPrior.add(PriorFactor<Pose3>(firstKey, Pose3(), priorModel));
|
||||||
|
break;
|
||||||
|
}
|
||||||
|
|
||||||
|
std::cout << "Optimizing the factor graph" << std::endl;
|
||||||
|
GaussNewtonParams params;
|
||||||
|
params.setVerbosity("TERMINATION"); // this will show info about stopping conditions
|
||||||
|
GaussNewtonOptimizer optimizer(graphWithPrior, *initial, params);
|
||||||
|
Values result = optimizer.optimize();
|
||||||
|
std::cout << "Optimization complete" << std::endl;
|
||||||
|
|
||||||
|
std::cout << "initial error=" <<graph->error(*initial)<< std::endl;
|
||||||
|
std::cout << "final error=" <<graph->error(result)<< std::endl;
|
||||||
|
|
||||||
|
if (argc < 3) {
|
||||||
|
result.print("result");
|
||||||
|
} else {
|
||||||
|
const string outputFile = argv[2];
|
||||||
|
std::cout << "Writing results to file: " << outputFile << std::endl;
|
||||||
|
writeG2o(*graph, result, outputFile);
|
||||||
|
std::cout << "done! " << std::endl;
|
||||||
|
}
|
||||||
|
return 0;
|
||||||
|
}
|
||||||
|
|
@ -0,0 +1,68 @@
|
||||||
|
/* ----------------------------------------------------------------------------
|
||||||
|
|
||||||
|
* GTSAM Copyright 2010, Georgia Tech Research Corporation,
|
||||||
|
* Atlanta, Georgia 30332-0415
|
||||||
|
* All Rights Reserved
|
||||||
|
* Authors: Frank Dellaert, et al. (see THANKS for the full author list)
|
||||||
|
|
||||||
|
* See LICENSE for the license information
|
||||||
|
|
||||||
|
* -------------------------------------------------------------------------- */
|
||||||
|
|
||||||
|
/**
|
||||||
|
* @file Pose3SLAMExample_initializePose3.cpp
|
||||||
|
* @brief A 3D Pose SLAM example that reads input from g2o, and initializes the Pose3 using InitializePose3
|
||||||
|
* Syntax for the script is ./Pose3SLAMExample_initializePose3 input.g2o output.g2o
|
||||||
|
* @date Aug 25, 2014
|
||||||
|
* @author Luca Carlone
|
||||||
|
*/
|
||||||
|
|
||||||
|
#include <gtsam/slam/InitializePose3.h>
|
||||||
|
#include <gtsam/slam/dataset.h>
|
||||||
|
#include <gtsam/slam/BetweenFactor.h>
|
||||||
|
#include <gtsam/slam/PriorFactor.h>
|
||||||
|
#include <fstream>
|
||||||
|
|
||||||
|
using namespace std;
|
||||||
|
using namespace gtsam;
|
||||||
|
|
||||||
|
int main(const int argc, const char *argv[]) {
|
||||||
|
|
||||||
|
// Read graph from file
|
||||||
|
string g2oFile;
|
||||||
|
if (argc < 2)
|
||||||
|
g2oFile = findExampleDataFile("pose3example.txt");
|
||||||
|
else
|
||||||
|
g2oFile = argv[1];
|
||||||
|
|
||||||
|
NonlinearFactorGraph::shared_ptr graph;
|
||||||
|
Values::shared_ptr initial;
|
||||||
|
bool is3D = true;
|
||||||
|
boost::tie(graph, initial) = readG2o(g2oFile, is3D);
|
||||||
|
|
||||||
|
// Add prior on the first key
|
||||||
|
NonlinearFactorGraph graphWithPrior = *graph;
|
||||||
|
noiseModel::Diagonal::shared_ptr priorModel = //
|
||||||
|
noiseModel::Diagonal::Variances((Vector(6) << 1e-6, 1e-6, 1e-6, 1e-4, 1e-4, 1e-4));
|
||||||
|
Key firstKey = 0;
|
||||||
|
BOOST_FOREACH(const Values::ConstKeyValuePair& key_value, *initial) {
|
||||||
|
std::cout << "Adding prior to g2o file " << std::endl;
|
||||||
|
firstKey = key_value.key;
|
||||||
|
graphWithPrior.add(PriorFactor<Pose3>(firstKey, Pose3(), priorModel));
|
||||||
|
break;
|
||||||
|
}
|
||||||
|
|
||||||
|
std::cout << "Initializing Pose3 - chordal relaxation" << std::endl;
|
||||||
|
Values initialization = InitializePose3::initialize(graphWithPrior);
|
||||||
|
std::cout << "done!" << std::endl;
|
||||||
|
|
||||||
|
if (argc < 3) {
|
||||||
|
initialization.print("initialization");
|
||||||
|
} else {
|
||||||
|
const string outputFile = argv[2];
|
||||||
|
std::cout << "Writing results to file: " << outputFile << std::endl;
|
||||||
|
writeG2o(*graph, initialization, outputFile);
|
||||||
|
std::cout << "done! " << std::endl;
|
||||||
|
}
|
||||||
|
return 0;
|
||||||
|
}
|
||||||
|
|
@ -0,0 +1,72 @@
|
||||||
|
/* ----------------------------------------------------------------------------
|
||||||
|
|
||||||
|
* GTSAM Copyright 2010, Georgia Tech Research Corporation,
|
||||||
|
* Atlanta, Georgia 30332-0415
|
||||||
|
* All Rights Reserved
|
||||||
|
* Authors: Frank Dellaert, et al. (see THANKS for the full author list)
|
||||||
|
|
||||||
|
* See LICENSE for the license information
|
||||||
|
|
||||||
|
* -------------------------------------------------------------------------- */
|
||||||
|
|
||||||
|
/**
|
||||||
|
* @file Pose3SLAMExample_initializePose3.cpp
|
||||||
|
* @brief A 3D Pose SLAM example that reads input from g2o, and initializes the Pose3 using InitializePose3
|
||||||
|
* Syntax for the script is ./Pose3SLAMExample_initializePose3 input.g2o output.g2o
|
||||||
|
* @date Aug 25, 2014
|
||||||
|
* @author Luca Carlone
|
||||||
|
*/
|
||||||
|
|
||||||
|
#include <gtsam/slam/InitializePose3.h>
|
||||||
|
#include <gtsam/slam/dataset.h>
|
||||||
|
#include <gtsam/slam/BetweenFactor.h>
|
||||||
|
#include <gtsam/slam/PriorFactor.h>
|
||||||
|
#include <fstream>
|
||||||
|
|
||||||
|
using namespace std;
|
||||||
|
using namespace gtsam;
|
||||||
|
|
||||||
|
int main(const int argc, const char *argv[]) {
|
||||||
|
|
||||||
|
// Read graph from file
|
||||||
|
string g2oFile;
|
||||||
|
if (argc < 2)
|
||||||
|
g2oFile = findExampleDataFile("pose3example.txt");
|
||||||
|
else
|
||||||
|
g2oFile = argv[1];
|
||||||
|
|
||||||
|
NonlinearFactorGraph::shared_ptr graph;
|
||||||
|
Values::shared_ptr initial;
|
||||||
|
bool is3D = true;
|
||||||
|
boost::tie(graph, initial) = readG2o(g2oFile, is3D);
|
||||||
|
|
||||||
|
// Add prior on the first key
|
||||||
|
NonlinearFactorGraph graphWithPrior = *graph;
|
||||||
|
noiseModel::Diagonal::shared_ptr priorModel = //
|
||||||
|
noiseModel::Diagonal::Variances((Vector(6) << 1e-6, 1e-6, 1e-6, 1e-4, 1e-4, 1e-4));
|
||||||
|
Key firstKey = 0;
|
||||||
|
BOOST_FOREACH(const Values::ConstKeyValuePair& key_value, *initial) {
|
||||||
|
std::cout << "Adding prior to g2o file " << std::endl;
|
||||||
|
firstKey = key_value.key;
|
||||||
|
graphWithPrior.add(PriorFactor<Pose3>(firstKey, Pose3(), priorModel));
|
||||||
|
break;
|
||||||
|
}
|
||||||
|
|
||||||
|
std::cout << "Initializing Pose3 - Riemannian gradient" << std::endl;
|
||||||
|
bool useGradient = true;
|
||||||
|
Values initialization = InitializePose3::initialize(graphWithPrior, *initial, useGradient);
|
||||||
|
std::cout << "done!" << std::endl;
|
||||||
|
|
||||||
|
std::cout << "initial error=" <<graph->error(*initial)<< std::endl;
|
||||||
|
std::cout << "initialization error=" <<graph->error(initialization)<< std::endl;
|
||||||
|
|
||||||
|
if (argc < 3) {
|
||||||
|
initialization.print("initialization");
|
||||||
|
} else {
|
||||||
|
const string outputFile = argv[2];
|
||||||
|
std::cout << "Writing results to file: " << outputFile << std::endl;
|
||||||
|
writeG2o(*graph, initialization, outputFile);
|
||||||
|
std::cout << "done! " << std::endl;
|
||||||
|
}
|
||||||
|
return 0;
|
||||||
|
}
|
||||||
|
|
@ -0,0 +1,410 @@
|
||||||
|
/* ----------------------------------------------------------------------------
|
||||||
|
|
||||||
|
* GTSAM Copyright 2010, Georgia Tech Research Corporation,
|
||||||
|
* Atlanta, Georgia 30332-0415
|
||||||
|
* All Rights Reserved
|
||||||
|
* Authors: Frank Dellaert, et al. (see THANKS for the full author list)
|
||||||
|
|
||||||
|
* See LICENSE for the license information
|
||||||
|
|
||||||
|
* -------------------------------------------------------------------------- */
|
||||||
|
|
||||||
|
/**
|
||||||
|
* @file InitializePose3.h
|
||||||
|
* @author Luca Carlone
|
||||||
|
* @author Frank Dellaert
|
||||||
|
* @date August, 2014
|
||||||
|
*/
|
||||||
|
|
||||||
|
#include <gtsam/slam/InitializePose3.h>
|
||||||
|
#include <gtsam/slam/PriorFactor.h>
|
||||||
|
#include <gtsam/slam/BetweenFactor.h>
|
||||||
|
#include <gtsam/nonlinear/GaussNewtonOptimizer.h>
|
||||||
|
#include <gtsam/inference/Symbol.h>
|
||||||
|
#include <gtsam/geometry/Pose3.h>
|
||||||
|
#include <gtsam/base/timing.h>
|
||||||
|
|
||||||
|
#include <boost/math/special_functions.hpp>
|
||||||
|
|
||||||
|
using namespace std;
|
||||||
|
|
||||||
|
namespace gtsam {
|
||||||
|
namespace InitializePose3 {
|
||||||
|
|
||||||
|
//static const Matrix I = eye(1);
|
||||||
|
static const Matrix I9 = eye(9);
|
||||||
|
static const Vector zero9 = Vector::Zero(9);
|
||||||
|
static const Matrix zero33= Matrix::Zero(3,3);
|
||||||
|
|
||||||
|
static const Key keyAnchor = symbol('Z', 9999999);
|
||||||
|
|
||||||
|
/* ************************************************************************* */
|
||||||
|
GaussianFactorGraph buildLinearOrientationGraph(const NonlinearFactorGraph& g) {
|
||||||
|
|
||||||
|
GaussianFactorGraph linearGraph;
|
||||||
|
noiseModel::Unit::shared_ptr model = noiseModel::Unit::Create(9);
|
||||||
|
|
||||||
|
BOOST_FOREACH(const boost::shared_ptr<NonlinearFactor>& factor, g) {
|
||||||
|
Matrix3 Rij;
|
||||||
|
|
||||||
|
boost::shared_ptr<BetweenFactor<Pose3> > pose3Between =
|
||||||
|
boost::dynamic_pointer_cast<BetweenFactor<Pose3> >(factor);
|
||||||
|
if (pose3Between)
|
||||||
|
Rij = pose3Between->measured().rotation().matrix();
|
||||||
|
else
|
||||||
|
std::cout << "Error in buildLinearOrientationGraph" << std::endl;
|
||||||
|
|
||||||
|
// std::cout << "Rij \n" << Rij << std::endl;
|
||||||
|
|
||||||
|
const FastVector<Key>& keys = factor->keys();
|
||||||
|
Key key1 = keys[0], key2 = keys[1];
|
||||||
|
Matrix M9 = Matrix::Zero(9,9);
|
||||||
|
M9.block(0,0,3,3) = Rij;
|
||||||
|
M9.block(3,3,3,3) = Rij;
|
||||||
|
M9.block(6,6,3,3) = Rij;
|
||||||
|
linearGraph.add(key1, -I9, key2, M9, zero9, model);
|
||||||
|
}
|
||||||
|
// prior on the anchor orientation
|
||||||
|
linearGraph.add(keyAnchor, I9, (Vector(9) << 1.0, 0.0, 0.0,/* */ 0.0, 1.0, 0.0, /* */ 0.0, 0.0, 1.0), model);
|
||||||
|
return linearGraph;
|
||||||
|
}
|
||||||
|
|
||||||
|
/* ************************************************************************* */
|
||||||
|
// Transform VectorValues into valid Rot3
|
||||||
|
Values normalizeRelaxedRotations(const VectorValues& relaxedRot3) {
|
||||||
|
gttic(InitializePose3_computeOrientationsChordal);
|
||||||
|
|
||||||
|
Matrix ppm = Matrix::Zero(3,3); // plus plus minus
|
||||||
|
ppm(0,0) = 1; ppm(1,1) = 1; ppm(2,2) = -1;
|
||||||
|
|
||||||
|
Values validRot3;
|
||||||
|
BOOST_FOREACH(const VectorValues::value_type& it, relaxedRot3) {
|
||||||
|
Key key = it.first;
|
||||||
|
if (key != keyAnchor) {
|
||||||
|
const Vector& rotVector = it.second;
|
||||||
|
Matrix3 rotMat;
|
||||||
|
rotMat(0,0) = rotVector(0); rotMat(0,1) = rotVector(1); rotMat(0,2) = rotVector(2);
|
||||||
|
rotMat(1,0) = rotVector(3); rotMat(1,1) = rotVector(4); rotMat(1,2) = rotVector(5);
|
||||||
|
rotMat(2,0) = rotVector(6); rotMat(2,1) = rotVector(7); rotMat(2,2) = rotVector(8);
|
||||||
|
|
||||||
|
Matrix U, V; Vector s;
|
||||||
|
svd(rotMat, U, s, V);
|
||||||
|
Matrix3 normalizedRotMat = U * V.transpose();
|
||||||
|
|
||||||
|
// std::cout << "rotMat \n" << rotMat << std::endl;
|
||||||
|
// std::cout << "U V' \n" << U * V.transpose() << std::endl;
|
||||||
|
// std::cout << "V \n" << V << std::endl;
|
||||||
|
|
||||||
|
if(normalizedRotMat.determinant() < 0)
|
||||||
|
normalizedRotMat = U * ppm * V.transpose();
|
||||||
|
|
||||||
|
Rot3 initRot = Rot3(normalizedRotMat);
|
||||||
|
validRot3.insert(key, initRot);
|
||||||
|
}
|
||||||
|
}
|
||||||
|
return validRot3;
|
||||||
|
}
|
||||||
|
|
||||||
|
/* ************************************************************************* */
|
||||||
|
// Select the subgraph of betweenFactors and transforms priors into between wrt a fictitious node
|
||||||
|
NonlinearFactorGraph buildPose3graph(const NonlinearFactorGraph& graph) {
|
||||||
|
gttic(InitializePose3_buildPose3graph);
|
||||||
|
NonlinearFactorGraph pose3Graph;
|
||||||
|
|
||||||
|
BOOST_FOREACH(const boost::shared_ptr<NonlinearFactor>& factor, graph) {
|
||||||
|
|
||||||
|
// recast to a between on Pose3
|
||||||
|
boost::shared_ptr<BetweenFactor<Pose3> > pose3Between =
|
||||||
|
boost::dynamic_pointer_cast<BetweenFactor<Pose3> >(factor);
|
||||||
|
if (pose3Between)
|
||||||
|
pose3Graph.add(pose3Between);
|
||||||
|
|
||||||
|
// recast PriorFactor<Pose3> to BetweenFactor<Pose3>
|
||||||
|
boost::shared_ptr<PriorFactor<Pose3> > pose3Prior =
|
||||||
|
boost::dynamic_pointer_cast<PriorFactor<Pose3> >(factor);
|
||||||
|
if (pose3Prior)
|
||||||
|
pose3Graph.add(
|
||||||
|
BetweenFactor<Pose3>(keyAnchor, pose3Prior->keys()[0],
|
||||||
|
pose3Prior->prior(), pose3Prior->get_noiseModel()));
|
||||||
|
}
|
||||||
|
return pose3Graph;
|
||||||
|
}
|
||||||
|
|
||||||
|
/* ************************************************************************* */
|
||||||
|
// Return the orientations of a graph including only BetweenFactors<Pose3>
|
||||||
|
Values computeOrientationsChordal(const NonlinearFactorGraph& pose3Graph) {
|
||||||
|
gttic(InitializePose3_computeOrientationsChordal);
|
||||||
|
|
||||||
|
// regularize measurements and plug everything in a factor graph
|
||||||
|
GaussianFactorGraph relaxedGraph = buildLinearOrientationGraph(pose3Graph);
|
||||||
|
|
||||||
|
// Solve the LFG
|
||||||
|
VectorValues relaxedRot3 = relaxedGraph.optimize();
|
||||||
|
|
||||||
|
// normalize and compute Rot3
|
||||||
|
return normalizeRelaxedRotations(relaxedRot3);
|
||||||
|
}
|
||||||
|
|
||||||
|
/* ************************************************************************* */
|
||||||
|
// Return the orientations of a graph including only BetweenFactors<Pose3>
|
||||||
|
Values computeOrientationsGradient(const NonlinearFactorGraph& pose3Graph, const Values& givenGuess, const size_t maxIter, const bool setRefFrame) {
|
||||||
|
gttic(InitializePose3_computeOrientationsGradient);
|
||||||
|
|
||||||
|
// this works on the inverse rotations, according to Tron&Vidal,2011
|
||||||
|
Values inverseRot;
|
||||||
|
inverseRot.insert(keyAnchor, Rot3());
|
||||||
|
BOOST_FOREACH(const Values::ConstKeyValuePair& key_value, givenGuess) {
|
||||||
|
Key key = key_value.key;
|
||||||
|
const Pose3& pose = givenGuess.at<Pose3>(key);
|
||||||
|
inverseRot.insert(key, pose.rotation().inverse());
|
||||||
|
}
|
||||||
|
|
||||||
|
// Create the map of edges incident on each node
|
||||||
|
KeyVectorMap adjEdgesMap;
|
||||||
|
KeyRotMap factorId2RotMap;
|
||||||
|
|
||||||
|
createSymbolicGraph(adjEdgesMap, factorId2RotMap, pose3Graph);
|
||||||
|
|
||||||
|
// calculate max node degree & allocate gradient
|
||||||
|
size_t maxNodeDeg = 0;
|
||||||
|
VectorValues grad;
|
||||||
|
BOOST_FOREACH(const Values::ConstKeyValuePair& key_value, inverseRot) {
|
||||||
|
Key key = key_value.key;
|
||||||
|
grad.insert(key,Vector3::Zero());
|
||||||
|
size_t currNodeDeg = (adjEdgesMap.at(key)).size();
|
||||||
|
if(currNodeDeg > maxNodeDeg)
|
||||||
|
maxNodeDeg = currNodeDeg;
|
||||||
|
}
|
||||||
|
|
||||||
|
// Create parameters
|
||||||
|
double b = 1;
|
||||||
|
double f0 = 1/b - (1/b + M_PI) * exp(-b*M_PI);
|
||||||
|
double a = (M_PI*M_PI)/(2*f0);
|
||||||
|
double rho = 2*a*b;
|
||||||
|
double mu_max = maxNodeDeg * rho;
|
||||||
|
double stepsize = 2/mu_max; // = 1/(a b dG)
|
||||||
|
|
||||||
|
std::cout <<" b " << b <<" f0 " << f0 <<" a " << a <<" rho " << rho <<" stepsize " << stepsize << " maxNodeDeg "<< maxNodeDeg << std::endl;
|
||||||
|
double maxGrad;
|
||||||
|
// gradient iterations
|
||||||
|
size_t it;
|
||||||
|
for(it=0; it < maxIter; it++){
|
||||||
|
//////////////////////////////////////////////////////////////////////////
|
||||||
|
// compute the gradient at each node
|
||||||
|
//std::cout << "it " << it <<" b " << b <<" f0 " << f0 <<" a " << a
|
||||||
|
// <<" rho " << rho <<" stepsize " << stepsize << " maxNodeDeg "<< maxNodeDeg << std::endl;
|
||||||
|
maxGrad = 0;
|
||||||
|
BOOST_FOREACH(const Values::ConstKeyValuePair& key_value, inverseRot) {
|
||||||
|
Key key = key_value.key;
|
||||||
|
//std::cout << "---------------------------key " << DefaultKeyFormatter(key) << std::endl;
|
||||||
|
Vector gradKey = Vector3::Zero();
|
||||||
|
// collect the gradient for each edge incident on key
|
||||||
|
BOOST_FOREACH(const size_t& factorId, adjEdgesMap.at(key)){
|
||||||
|
Rot3 Rij = factorId2RotMap.at(factorId);
|
||||||
|
Rot3 Ri = inverseRot.at<Rot3>(key);
|
||||||
|
if( key == (pose3Graph.at(factorId))->keys()[0] ){
|
||||||
|
Key key1 = (pose3Graph.at(factorId))->keys()[1];
|
||||||
|
Rot3 Rj = inverseRot.at<Rot3>(key1);
|
||||||
|
gradKey = gradKey + gradientTron(Ri, Rij * Rj, a, b);
|
||||||
|
//std::cout << "key1 " << DefaultKeyFormatter(key1) << " gradientTron(Ri, Rij * Rj, a, b) \n " << gradientTron(Ri, Rij * Rj, a, b) << std::endl;
|
||||||
|
}else if( key == (pose3Graph.at(factorId))->keys()[1] ){
|
||||||
|
Key key0 = (pose3Graph.at(factorId))->keys()[0];
|
||||||
|
Rot3 Rj = inverseRot.at<Rot3>(key0);
|
||||||
|
gradKey = gradKey + gradientTron(Ri, Rij.between(Rj), a, b);
|
||||||
|
//std::cout << "key0 " << DefaultKeyFormatter(key0) << " gradientTron(Ri, Rij.inverse() * Rj, a, b) \n " << gradientTron(Ri, Rij.between(Rj), a, b) << std::endl;
|
||||||
|
}else{
|
||||||
|
std::cout << "Error in gradient computation" << std::endl;
|
||||||
|
}
|
||||||
|
} // end of i-th gradient computation
|
||||||
|
grad.at(key) = stepsize * gradKey;
|
||||||
|
|
||||||
|
double normGradKey = (gradKey).norm();
|
||||||
|
//std::cout << "key " << DefaultKeyFormatter(key) <<" \n grad \n" << grad.at(key) << std::endl;
|
||||||
|
if(normGradKey>maxGrad)
|
||||||
|
maxGrad = normGradKey;
|
||||||
|
} // end of loop over nodes
|
||||||
|
|
||||||
|
//////////////////////////////////////////////////////////////////////////
|
||||||
|
// update estimates
|
||||||
|
inverseRot = inverseRot.retract(grad);
|
||||||
|
|
||||||
|
//////////////////////////////////////////////////////////////////////////
|
||||||
|
// check stopping condition
|
||||||
|
if (it>20 && maxGrad < 5e-3)
|
||||||
|
break;
|
||||||
|
} // enf of gradient iterations
|
||||||
|
|
||||||
|
std::cout << "nr of gradient iterations " << it << "maxGrad " << maxGrad << std::endl;
|
||||||
|
|
||||||
|
// Return correct rotations
|
||||||
|
const Rot3& Rref = inverseRot.at<Rot3>(keyAnchor); // This will be set to the identity as so far we included no prior
|
||||||
|
Values estimateRot;
|
||||||
|
BOOST_FOREACH(const Values::ConstKeyValuePair& key_value, inverseRot) {
|
||||||
|
Key key = key_value.key;
|
||||||
|
if (key != keyAnchor) {
|
||||||
|
const Rot3& R = inverseRot.at<Rot3>(key);
|
||||||
|
if(setRefFrame)
|
||||||
|
estimateRot.insert(key, Rref.compose(R.inverse()));
|
||||||
|
else
|
||||||
|
estimateRot.insert(key, R.inverse());
|
||||||
|
}
|
||||||
|
}
|
||||||
|
return estimateRot;
|
||||||
|
}
|
||||||
|
|
||||||
|
/* ************************************************************************* */
|
||||||
|
void createSymbolicGraph(KeyVectorMap& adjEdgesMap, KeyRotMap& factorId2RotMap, const NonlinearFactorGraph& pose3Graph){
|
||||||
|
size_t factorId = 0;
|
||||||
|
BOOST_FOREACH(const boost::shared_ptr<NonlinearFactor>& factor, pose3Graph) {
|
||||||
|
boost::shared_ptr<BetweenFactor<Pose3> > pose3Between =
|
||||||
|
boost::dynamic_pointer_cast<BetweenFactor<Pose3> >(factor);
|
||||||
|
if (pose3Between){
|
||||||
|
Rot3 Rij = pose3Between->measured().rotation();
|
||||||
|
factorId2RotMap.insert(pair<Key, Rot3 >(factorId,Rij));
|
||||||
|
|
||||||
|
Key key1 = pose3Between->key1();
|
||||||
|
if (adjEdgesMap.find(key1) != adjEdgesMap.end()){ // key is already in
|
||||||
|
adjEdgesMap.at(key1).push_back(factorId);
|
||||||
|
}else{
|
||||||
|
vector<size_t> edge_id;
|
||||||
|
edge_id.push_back(factorId);
|
||||||
|
adjEdgesMap.insert(pair<Key, vector<size_t> >(key1, edge_id));
|
||||||
|
}
|
||||||
|
Key key2 = pose3Between->key2();
|
||||||
|
if (adjEdgesMap.find(key2) != adjEdgesMap.end()){ // key is already in
|
||||||
|
adjEdgesMap.at(key2).push_back(factorId);
|
||||||
|
}else{
|
||||||
|
vector<size_t> edge_id;
|
||||||
|
edge_id.push_back(factorId);
|
||||||
|
adjEdgesMap.insert(pair<Key, vector<size_t> >(key2, edge_id));
|
||||||
|
}
|
||||||
|
}else{
|
||||||
|
std::cout << "Error in computeOrientationsGradient" << std::endl;
|
||||||
|
}
|
||||||
|
factorId++;
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
/* ************************************************************************* */
|
||||||
|
Vector3 gradientTron(const Rot3& R1, const Rot3& R2, const double a, const double b) {
|
||||||
|
Vector3 logRot = Rot3::Logmap(R1.between(R2));
|
||||||
|
|
||||||
|
double th = logRot.norm();
|
||||||
|
if(th != th){ // the second case means that th = nan (logRot does not work well for +/-pi)
|
||||||
|
Rot3 R1pert = R1.compose( Rot3::Expmap((Vector(3)<< 0.01, 0.01, 0.01)) ); // some perturbation
|
||||||
|
logRot = Rot3::Logmap(R1pert.between(R2));
|
||||||
|
th = logRot.norm();
|
||||||
|
}
|
||||||
|
// exclude small or invalid rotations
|
||||||
|
if (th > 1e-5 && th == th){ // nonzero valid rotations
|
||||||
|
logRot = logRot / th;
|
||||||
|
}else{
|
||||||
|
logRot = Vector3::Zero();
|
||||||
|
th = 0.0;
|
||||||
|
}
|
||||||
|
|
||||||
|
double fdot = a*b*th*exp(-b*th);
|
||||||
|
return fdot*logRot;
|
||||||
|
}
|
||||||
|
|
||||||
|
/* ************************************************************************* */
|
||||||
|
Values initializeOrientations(const NonlinearFactorGraph& graph) {
|
||||||
|
|
||||||
|
// We "extract" the Pose3 subgraph of the original graph: this
|
||||||
|
// is done to properly model priors and avoiding operating on a larger graph
|
||||||
|
NonlinearFactorGraph pose3Graph = buildPose3graph(graph);
|
||||||
|
|
||||||
|
// Get orientations from relative orientation measurements
|
||||||
|
return computeOrientationsChordal(pose3Graph);
|
||||||
|
}
|
||||||
|
|
||||||
|
///* ************************************************************************* */
|
||||||
|
Values computePoses(NonlinearFactorGraph& pose3graph, Values& initialRot) {
|
||||||
|
gttic(InitializePose3_computePoses);
|
||||||
|
|
||||||
|
// put into Values structure
|
||||||
|
Values initialPose;
|
||||||
|
BOOST_FOREACH(const Values::ConstKeyValuePair& key_value, initialRot){
|
||||||
|
Key key = key_value.key;
|
||||||
|
const Rot3& rot = initialRot.at<Rot3>(key);
|
||||||
|
Pose3 initializedPose = Pose3(rot, Point3());
|
||||||
|
initialPose.insert(key, initializedPose);
|
||||||
|
}
|
||||||
|
// add prior
|
||||||
|
noiseModel::Unit::shared_ptr priorModel = noiseModel::Unit::Create(6);
|
||||||
|
initialPose.insert(keyAnchor, Pose3());
|
||||||
|
pose3graph.add(PriorFactor<Pose3>(keyAnchor, Pose3(), priorModel));
|
||||||
|
|
||||||
|
// Create optimizer
|
||||||
|
GaussNewtonParams params;
|
||||||
|
bool singleIter = true;
|
||||||
|
if(singleIter){
|
||||||
|
params.maxIterations = 1;
|
||||||
|
}else{
|
||||||
|
std::cout << " \n\n\n\n performing more than 1 GN iterations \n\n\n" <<std::endl;
|
||||||
|
params.setVerbosity("TERMINATION");
|
||||||
|
}
|
||||||
|
GaussNewtonOptimizer optimizer(pose3graph, initialPose, params);
|
||||||
|
Values GNresult = optimizer.optimize();
|
||||||
|
|
||||||
|
// put into Values structure
|
||||||
|
Values estimate;
|
||||||
|
BOOST_FOREACH(const Values::ConstKeyValuePair& key_value, GNresult) {
|
||||||
|
Key key = key_value.key;
|
||||||
|
if (key != keyAnchor) {
|
||||||
|
const Pose3& pose = GNresult.at<Pose3>(key);
|
||||||
|
estimate.insert(key, pose);
|
||||||
|
}
|
||||||
|
}
|
||||||
|
return estimate;
|
||||||
|
}
|
||||||
|
|
||||||
|
/* ************************************************************************* */
|
||||||
|
Values initialize(const NonlinearFactorGraph& graph) {
|
||||||
|
gttic(InitializePose3_initialize);
|
||||||
|
|
||||||
|
// We "extract" the Pose3 subgraph of the original graph: this
|
||||||
|
// is done to properly model priors and avoiding operating on a larger graph
|
||||||
|
NonlinearFactorGraph pose3Graph = buildPose3graph(graph);
|
||||||
|
|
||||||
|
// Get orientations from relative orientation measurements
|
||||||
|
Values valueRot3 = computeOrientationsChordal(pose3Graph);
|
||||||
|
|
||||||
|
// Compute the full poses (1 GN iteration on full poses)
|
||||||
|
return computePoses(pose3Graph, valueRot3);
|
||||||
|
}
|
||||||
|
|
||||||
|
/* ************************************************************************* */
|
||||||
|
Values initialize(const NonlinearFactorGraph& graph, const Values& givenGuess, bool useGradient) {
|
||||||
|
Values initialValues;
|
||||||
|
|
||||||
|
// We "extract" the Pose3 subgraph of the original graph: this
|
||||||
|
// is done to properly model priors and avoiding operating on a larger graph
|
||||||
|
NonlinearFactorGraph pose3Graph = buildPose3graph(graph);
|
||||||
|
|
||||||
|
// Get orientations from relative orientation measurements
|
||||||
|
Values orientations;
|
||||||
|
if(useGradient)
|
||||||
|
orientations = computeOrientationsGradient(pose3Graph, givenGuess);
|
||||||
|
else
|
||||||
|
orientations = computeOrientationsChordal(pose3Graph);
|
||||||
|
|
||||||
|
// orientations.print("orientations\n");
|
||||||
|
|
||||||
|
// Compute the full poses (1 GN iteration on full poses)
|
||||||
|
return computePoses(pose3Graph, orientations);
|
||||||
|
|
||||||
|
// BOOST_FOREACH(const Values::ConstKeyValuePair& key_value, orientations) {
|
||||||
|
// Key key = key_value.key;
|
||||||
|
// if (key != keyAnchor) {
|
||||||
|
// const Point3& pos = givenGuess.at<Pose3>(key).translation();
|
||||||
|
// const Rot3& rot = orientations.at<Rot3>(key);
|
||||||
|
// Pose3 initializedPoses = Pose3(rot, pos);
|
||||||
|
// initialValues.insert(key, initializedPoses);
|
||||||
|
// }
|
||||||
|
// }
|
||||||
|
// return initialValues;
|
||||||
|
}
|
||||||
|
|
||||||
|
} // end of namespace lago
|
||||||
|
} // end of namespace gtsam
|
||||||
|
|
@ -0,0 +1,59 @@
|
||||||
|
/* ----------------------------------------------------------------------------
|
||||||
|
|
||||||
|
* GTSAM Copyright 2010, Georgia Tech Research Corporation,
|
||||||
|
* Atlanta, Georgia 30332-0415
|
||||||
|
* All Rights Reserved
|
||||||
|
* Authors: Frank Dellaert, et al. (see THANKS for the full author list)
|
||||||
|
|
||||||
|
* See LICENSE for the license information
|
||||||
|
|
||||||
|
* -------------------------------------------------------------------------- */
|
||||||
|
|
||||||
|
/**
|
||||||
|
* @file InitializePose3.h
|
||||||
|
* @brief Initialize Pose3 in a factor graph
|
||||||
|
*
|
||||||
|
* @author Luca Carlone
|
||||||
|
* @author Frank Dellaert
|
||||||
|
* @date August, 2014
|
||||||
|
*/
|
||||||
|
|
||||||
|
#pragma once
|
||||||
|
|
||||||
|
#include <gtsam/nonlinear/NonlinearFactorGraph.h>
|
||||||
|
#include <gtsam/linear/GaussianFactorGraph.h>
|
||||||
|
#include <gtsam/linear/VectorValues.h>
|
||||||
|
#include <gtsam/inference/graph.h>
|
||||||
|
#include <gtsam/geometry/Rot3.h>
|
||||||
|
|
||||||
|
namespace gtsam {
|
||||||
|
|
||||||
|
typedef std::map<Key, std::vector<size_t> > KeyVectorMap;
|
||||||
|
typedef std::map<Key, Rot3 > KeyRotMap;
|
||||||
|
|
||||||
|
namespace InitializePose3 {
|
||||||
|
|
||||||
|
GTSAM_EXPORT GaussianFactorGraph buildLinearOrientationGraph(const NonlinearFactorGraph& g);
|
||||||
|
|
||||||
|
GTSAM_EXPORT Values normalizeRelaxedRotations(const VectorValues& relaxedRot3);
|
||||||
|
|
||||||
|
GTSAM_EXPORT Values computeOrientationsChordal(const NonlinearFactorGraph& pose3Graph);
|
||||||
|
|
||||||
|
GTSAM_EXPORT Values computeOrientationsGradient(const NonlinearFactorGraph& pose3Graph,
|
||||||
|
const Values& givenGuess, size_t maxIter = 10000, const bool setRefFrame = true);
|
||||||
|
|
||||||
|
GTSAM_EXPORT void createSymbolicGraph(KeyVectorMap& adjEdgesMap, KeyRotMap& factorId2RotMap,
|
||||||
|
const NonlinearFactorGraph& pose3Graph);
|
||||||
|
|
||||||
|
GTSAM_EXPORT Vector3 gradientTron(const Rot3& R1, const Rot3& R2, const double a, const double b);
|
||||||
|
|
||||||
|
GTSAM_EXPORT NonlinearFactorGraph buildPose3graph(const NonlinearFactorGraph& graph);
|
||||||
|
|
||||||
|
GTSAM_EXPORT Values computePoses(NonlinearFactorGraph& pose3graph, Values& initialRot);
|
||||||
|
|
||||||
|
GTSAM_EXPORT Values initialize(const NonlinearFactorGraph& graph);
|
||||||
|
|
||||||
|
GTSAM_EXPORT Values initialize(const NonlinearFactorGraph& graph, const Values& givenGuess, bool useGradient = false);
|
||||||
|
|
||||||
|
} // end of namespace lago
|
||||||
|
} // end of namespace gtsam
|
||||||
|
|
@ -0,0 +1,259 @@
|
||||||
|
/* ----------------------------------------------------------------------------
|
||||||
|
|
||||||
|
* GTSAM Copyright 2010, Georgia Tech Research Corporation,
|
||||||
|
* Atlanta, Georgia 30332-0415
|
||||||
|
* All Rights Reserved
|
||||||
|
* Authors: Frank Dellaert, et al. (see THANKS for the full author list)
|
||||||
|
|
||||||
|
* See LICENSE for the license information
|
||||||
|
|
||||||
|
* -------------------------------------------------------------------------- */
|
||||||
|
|
||||||
|
/**
|
||||||
|
* @file testInitializePose3.cpp
|
||||||
|
* @brief Unit tests for 3D SLAM initialization, using rotation relaxation
|
||||||
|
*
|
||||||
|
* @author Luca Carlone
|
||||||
|
* @author Frank Dellaert
|
||||||
|
* @date August, 2014
|
||||||
|
*/
|
||||||
|
|
||||||
|
#include <gtsam/slam/InitializePose3.h>
|
||||||
|
#include <gtsam/slam/dataset.h>
|
||||||
|
#include <gtsam/slam/BetweenFactor.h>
|
||||||
|
#include <gtsam/slam/PriorFactor.h>
|
||||||
|
#include <gtsam/inference/Symbol.h>
|
||||||
|
#include <gtsam/geometry/Pose3.h>
|
||||||
|
#include <CppUnitLite/TestHarness.h>
|
||||||
|
|
||||||
|
#include <cmath>
|
||||||
|
|
||||||
|
using namespace std;
|
||||||
|
using namespace gtsam;
|
||||||
|
using namespace boost::assign;
|
||||||
|
|
||||||
|
static Symbol x0('x', 0), x1('x', 1), x2('x', 2), x3('x', 3);
|
||||||
|
static SharedNoiseModel model(noiseModel::Isotropic::Sigma(6, 0.1));
|
||||||
|
|
||||||
|
namespace simple {
|
||||||
|
// We consider a small graph:
|
||||||
|
// symbolic FG
|
||||||
|
// x2 0 1
|
||||||
|
// / | \ 1 2
|
||||||
|
// / | \ 2 3
|
||||||
|
// x3 | x1 2 0
|
||||||
|
// \ | / 0 3
|
||||||
|
// \ | /
|
||||||
|
// x0
|
||||||
|
//
|
||||||
|
static Point3 p0 = Point3(0,0,0);
|
||||||
|
static Rot3 R0 = Rot3::Expmap( ( Vector(3) << 0.0,0.0,0.0 ) );
|
||||||
|
static Point3 p1 = Point3(1,2,0);
|
||||||
|
static Rot3 R1 = Rot3::Expmap( ( Vector(3) << 0.0,0.0,1.570796 ) );
|
||||||
|
static Point3 p2 = Point3(0,2,0);
|
||||||
|
static Rot3 R2 = Rot3::Expmap( ( Vector(3) << 0.0,0.0,3.141593 ) );
|
||||||
|
static Point3 p3 = Point3(-1,1,0);
|
||||||
|
static Rot3 R3 = Rot3::Expmap( ( Vector(3) << 0.0,0.0,4.712389 ) );
|
||||||
|
|
||||||
|
static Pose3 pose0 = Pose3(R0,p0);
|
||||||
|
static Pose3 pose1 = Pose3(R1,p1);
|
||||||
|
static Pose3 pose2 = Pose3(R2,p2);
|
||||||
|
static Pose3 pose3 = Pose3(R3,p3);
|
||||||
|
|
||||||
|
NonlinearFactorGraph graph() {
|
||||||
|
NonlinearFactorGraph g;
|
||||||
|
g.add(BetweenFactor<Pose3>(x0, x1, pose0.between(pose1), model));
|
||||||
|
g.add(BetweenFactor<Pose3>(x1, x2, pose1.between(pose2), model));
|
||||||
|
g.add(BetweenFactor<Pose3>(x2, x3, pose2.between(pose3), model));
|
||||||
|
g.add(BetweenFactor<Pose3>(x2, x0, pose2.between(pose0), model));
|
||||||
|
g.add(BetweenFactor<Pose3>(x0, x3, pose0.between(pose3), model));
|
||||||
|
g.add(PriorFactor<Pose3>(x0, pose0, model));
|
||||||
|
return g;
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
/* *************************************************************************** */
|
||||||
|
TEST( InitializePose3, buildPose3graph ) {
|
||||||
|
NonlinearFactorGraph pose3graph = InitializePose3::buildPose3graph(simple::graph());
|
||||||
|
// pose3graph.print("");
|
||||||
|
}
|
||||||
|
|
||||||
|
/* *************************************************************************** */
|
||||||
|
TEST( InitializePose3, orientations ) {
|
||||||
|
NonlinearFactorGraph pose3Graph = InitializePose3::buildPose3graph(simple::graph());
|
||||||
|
|
||||||
|
Values initial = InitializePose3::computeOrientationsChordal(pose3Graph);
|
||||||
|
|
||||||
|
// comparison is up to M_PI, that's why we add some multiples of 2*M_PI
|
||||||
|
EXPECT(assert_equal(simple::R0, initial.at<Rot3>(x0), 1e-6));
|
||||||
|
EXPECT(assert_equal(simple::R1, initial.at<Rot3>(x1), 1e-6));
|
||||||
|
EXPECT(assert_equal(simple::R2, initial.at<Rot3>(x2), 1e-6));
|
||||||
|
EXPECT(assert_equal(simple::R3, initial.at<Rot3>(x3), 1e-6));
|
||||||
|
}
|
||||||
|
|
||||||
|
/* *************************************************************************** */
|
||||||
|
TEST( InitializePose3, orientationsGradientSymbolicGraph ) {
|
||||||
|
NonlinearFactorGraph pose3Graph = InitializePose3::buildPose3graph(simple::graph());
|
||||||
|
|
||||||
|
KeyVectorMap adjEdgesMap;
|
||||||
|
KeyRotMap factorId2RotMap;
|
||||||
|
|
||||||
|
InitializePose3::createSymbolicGraph(adjEdgesMap, factorId2RotMap, pose3Graph);
|
||||||
|
|
||||||
|
EXPECT_DOUBLES_EQUAL(adjEdgesMap.at(x0)[0], 0, 1e-9);
|
||||||
|
EXPECT_DOUBLES_EQUAL(adjEdgesMap.at(x0)[1], 3, 1e-9);
|
||||||
|
EXPECT_DOUBLES_EQUAL(adjEdgesMap.at(x0)[2], 4, 1e-9);
|
||||||
|
EXPECT_DOUBLES_EQUAL(adjEdgesMap.at(x0)[3], 5, 1e-9);
|
||||||
|
EXPECT_DOUBLES_EQUAL(adjEdgesMap.at(x0).size(), 4, 1e-9);
|
||||||
|
|
||||||
|
EXPECT_DOUBLES_EQUAL(adjEdgesMap.at(x1)[0], 0, 1e-9);
|
||||||
|
EXPECT_DOUBLES_EQUAL(adjEdgesMap.at(x1)[1], 1, 1e-9);
|
||||||
|
EXPECT_DOUBLES_EQUAL(adjEdgesMap.at(x1).size(), 2, 1e-9);
|
||||||
|
|
||||||
|
EXPECT_DOUBLES_EQUAL(adjEdgesMap.at(x2)[0], 1, 1e-9);
|
||||||
|
EXPECT_DOUBLES_EQUAL(adjEdgesMap.at(x2)[1], 2, 1e-9);
|
||||||
|
EXPECT_DOUBLES_EQUAL(adjEdgesMap.at(x2)[2], 3, 1e-9);
|
||||||
|
EXPECT_DOUBLES_EQUAL(adjEdgesMap.at(x2).size(), 3, 1e-9);
|
||||||
|
|
||||||
|
EXPECT_DOUBLES_EQUAL(adjEdgesMap.at(x3)[0], 2, 1e-9);
|
||||||
|
EXPECT_DOUBLES_EQUAL(adjEdgesMap.at(x3)[1], 4, 1e-9);
|
||||||
|
EXPECT_DOUBLES_EQUAL(adjEdgesMap.at(x3).size(), 2, 1e-9);
|
||||||
|
|
||||||
|
// This includes the anchor
|
||||||
|
EXPECT_DOUBLES_EQUAL(adjEdgesMap.size(), 5, 1e-9);
|
||||||
|
}
|
||||||
|
|
||||||
|
/* *************************************************************************** */
|
||||||
|
TEST( InitializePose3, singleGradient ) {
|
||||||
|
Rot3 R1 = Rot3();
|
||||||
|
Matrix M = Matrix3::Zero();
|
||||||
|
M(0,1) = -1; M(1,0) = 1; M(2,2) = 1;
|
||||||
|
Rot3 R2 = Rot3(M);
|
||||||
|
double a = 6.010534238540223;
|
||||||
|
double b = 1.0;
|
||||||
|
|
||||||
|
Vector actual = InitializePose3::gradientTron(R1, R2, a, b);
|
||||||
|
Vector expected = Vector3::Zero();
|
||||||
|
expected(2) = 1.962658662803917;
|
||||||
|
|
||||||
|
// comparison is up to M_PI, that's why we add some multiples of 2*M_PI
|
||||||
|
EXPECT(assert_equal(expected, actual, 1e-6));
|
||||||
|
}
|
||||||
|
|
||||||
|
/* *************************************************************************** */
|
||||||
|
TEST( InitializePose3, iterationGradient ) {
|
||||||
|
NonlinearFactorGraph pose3Graph = InitializePose3::buildPose3graph(simple::graph());
|
||||||
|
|
||||||
|
// Wrong initial guess - initialization should fix the rotations
|
||||||
|
Rot3 Rpert = Rot3::Expmap((Vector(3)<< 0.01, 0.01, 0.01));
|
||||||
|
Values givenPoses;
|
||||||
|
givenPoses.insert(x0,simple::pose0);
|
||||||
|
givenPoses.insert(x1,(simple::pose0).compose( Pose3(Rpert,Point3()) ));
|
||||||
|
givenPoses.insert(x2, (simple::pose0).compose( Pose3(Rpert.inverse(),Point3()) ));
|
||||||
|
givenPoses.insert(x3, (simple::pose0).compose( Pose3(Rpert,Point3()) ));
|
||||||
|
|
||||||
|
size_t maxIter = 1; // test gradient at the first iteration
|
||||||
|
bool setRefFrame = false;
|
||||||
|
Values orientations = InitializePose3::computeOrientationsGradient(pose3Graph, givenPoses, maxIter, setRefFrame);
|
||||||
|
|
||||||
|
Matrix M0 = (Matrix(3,3) << 0.999435813876064, -0.033571481675497, 0.001004768630281,
|
||||||
|
0.033572116359134, 0.999436104312325, -0.000621610948719,
|
||||||
|
-0.000983333645009, 0.000654992453817, 0.999999302019670);
|
||||||
|
Rot3 R0Expected = Rot3(M0);
|
||||||
|
EXPECT(assert_equal(R0Expected, orientations.at<Rot3>(x0), 1e-5));
|
||||||
|
|
||||||
|
Matrix M1 = (Matrix(3,3) << 0.999905367545392, -0.010866391403031, 0.008436675399114,
|
||||||
|
0.010943459008004, 0.999898317528125, -0.009143047050380,
|
||||||
|
-0.008336465609239, 0.009234508232789, 0.999922610604863);
|
||||||
|
Rot3 R1Expected = Rot3(M1);
|
||||||
|
EXPECT(assert_equal(R1Expected, orientations.at<Rot3>(x1), 1e-5));
|
||||||
|
|
||||||
|
Matrix M2 = (Matrix(3,3) << 0.998936644682875, 0.045376417678595, -0.008158469732553,
|
||||||
|
-0.045306446926148, 0.998936408933058, 0.008566024448664,
|
||||||
|
0.008538487960253, -0.008187284445083, 0.999930028850403);
|
||||||
|
Rot3 R2Expected = Rot3(M2);
|
||||||
|
EXPECT(assert_equal(R2Expected, orientations.at<Rot3>(x2), 1e-5));
|
||||||
|
|
||||||
|
Matrix M3 = (Matrix(3,3) << 0.999898767273093, -0.010834701971459, 0.009223038487275,
|
||||||
|
0.010911315499947, 0.999906044037258, -0.008297366559388,
|
||||||
|
-0.009132272433995, 0.008397162077148, 0.999923041673329);
|
||||||
|
Rot3 R3Expected = Rot3(M3);
|
||||||
|
EXPECT(assert_equal(R3Expected, orientations.at<Rot3>(x3), 1e-5));
|
||||||
|
}
|
||||||
|
|
||||||
|
/* *************************************************************************** */
|
||||||
|
TEST( InitializePose3, orientationsGradient ) {
|
||||||
|
NonlinearFactorGraph pose3Graph = InitializePose3::buildPose3graph(simple::graph());
|
||||||
|
|
||||||
|
// Wrong initial guess - initialization should fix the rotations
|
||||||
|
Rot3 Rpert = Rot3::Expmap((Vector(3)<< 0.01, 0.01, 0.01));
|
||||||
|
Values givenPoses;
|
||||||
|
givenPoses.insert(x0,simple::pose0);
|
||||||
|
givenPoses.insert(x1,(simple::pose0).compose( Pose3(Rpert,Point3()) ));
|
||||||
|
givenPoses.insert(x2, (simple::pose0).compose( Pose3(Rpert.inverse(),Point3()) ));
|
||||||
|
givenPoses.insert(x3, (simple::pose0).compose( Pose3(Rpert,Point3()) ));
|
||||||
|
// do 10 gradient iterations
|
||||||
|
bool setRefFrame = false;
|
||||||
|
Values orientations = InitializePose3::computeOrientationsGradient(pose3Graph, givenPoses, 10, setRefFrame);
|
||||||
|
|
||||||
|
// const Key keyAnchor = symbol('Z', 9999999);
|
||||||
|
// givenPoses.insert(keyAnchor,simple::pose0);
|
||||||
|
// string g2oFile = "/home/aspn/Desktop/toyExample.g2o";
|
||||||
|
// writeG2o(pose3Graph, givenPoses, g2oFile);
|
||||||
|
|
||||||
|
const string matlabResultsfile = findExampleDataFile("simpleGraph10gradIter");
|
||||||
|
NonlinearFactorGraph::shared_ptr matlabGraph;
|
||||||
|
Values::shared_ptr matlabValues;
|
||||||
|
bool is3D = true;
|
||||||
|
boost::tie(matlabGraph, matlabValues) = readG2o(matlabResultsfile, is3D);
|
||||||
|
|
||||||
|
Rot3 R0Expected = matlabValues->at<Pose3>(1).rotation();
|
||||||
|
EXPECT(assert_equal(R0Expected, orientations.at<Rot3>(x0), 1e-4));
|
||||||
|
|
||||||
|
Rot3 R1Expected = matlabValues->at<Pose3>(2).rotation();
|
||||||
|
EXPECT(assert_equal(R1Expected, orientations.at<Rot3>(x1), 1e-4));
|
||||||
|
|
||||||
|
Rot3 R2Expected = matlabValues->at<Pose3>(3).rotation();
|
||||||
|
EXPECT(assert_equal(R2Expected, orientations.at<Rot3>(x2), 1e-3));
|
||||||
|
|
||||||
|
Rot3 R3Expected = matlabValues->at<Pose3>(4).rotation();
|
||||||
|
EXPECT(assert_equal(R3Expected, orientations.at<Rot3>(x3), 1e-4));
|
||||||
|
}
|
||||||
|
|
||||||
|
/* *************************************************************************** */
|
||||||
|
TEST( InitializePose3, posesWithGivenGuess ) {
|
||||||
|
Values givenPoses;
|
||||||
|
givenPoses.insert(x0,simple::pose0);
|
||||||
|
givenPoses.insert(x1,simple::pose1);
|
||||||
|
givenPoses.insert(x2,simple::pose2);
|
||||||
|
givenPoses.insert(x3,simple::pose3);
|
||||||
|
|
||||||
|
Values initial = InitializePose3::initialize(simple::graph(), givenPoses);
|
||||||
|
|
||||||
|
// comparison is up to M_PI, that's why we add some multiples of 2*M_PI
|
||||||
|
EXPECT(assert_equal(givenPoses, initial, 1e-6));
|
||||||
|
}
|
||||||
|
|
||||||
|
/* ************************************************************************* */
|
||||||
|
TEST( InitializePose3, initializePoses )
|
||||||
|
{
|
||||||
|
const string g2oFile = findExampleDataFile("pose3example-grid");
|
||||||
|
NonlinearFactorGraph::shared_ptr inputGraph;
|
||||||
|
Values::shared_ptr expectedValues;
|
||||||
|
bool is3D = true;
|
||||||
|
boost::tie(inputGraph, expectedValues) = readG2o(g2oFile, is3D);
|
||||||
|
noiseModel::Unit::shared_ptr priorModel = noiseModel::Unit::Create(6);
|
||||||
|
inputGraph->add(PriorFactor<Pose3>(0, Pose3(), priorModel));
|
||||||
|
|
||||||
|
Values initial = InitializePose3::initialize(*inputGraph);
|
||||||
|
EXPECT(assert_equal(*expectedValues,initial,1e-4));
|
||||||
|
}
|
||||||
|
|
||||||
|
|
||||||
|
/* ************************************************************************* */
|
||||||
|
int main() {
|
||||||
|
TestResult tr;
|
||||||
|
return TestRegistry::runAllTests(tr);
|
||||||
|
}
|
||||||
|
/* ************************************************************************* */
|
||||||
|
|
||||||
Loading…
Reference in New Issue