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

Merge remote-tracking branch 'origin/develop' into feature/BAD 

Conflicts:
	.cproject
	gtsam/geometry/Cal3DS2.cpp
	gtsam/geometry/Cal3DS2.h
	gtsam/geometry/Cal3Unified.h
	gtsam/navigation/CombinedImuFactor.h
	gtsam/navigation/ImuFactor.h
	gtsam/nonlinear/NonlinearFactor.h
	gtsam/slam/tests/testPoseRotationPrior.cpp

Modified: testGaussMarkov1stOrderFactor.cpp, testPoseRotationPrior.cpp
release/4.3a0
dellaert 2014-11-04 17:04:57 +01:00
parent 7d57d91fec c212ba0984
commit a94835a2e4
64 changed files with 4318 additions and 2260 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

20
.cproject
View File

@ -848,18 +848,26 @@
<useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
</target>
<target name="testGaussianFactorGraph.run" path="build/gtsam/linear/tests" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<target name="testGaussMarkov1stOrderFactor.run" path="build/gtsam_unstable/slam/tests" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments>-j5</buildArguments>
<buildTarget>testGaussianFactorGraph.run</buildTarget>
<buildTarget>testGaussMarkov1stOrderFactor.run</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
</target>
<target name="testGaussianBayesNet.run" path="build/gtsam/linear/tests" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<target name="testGaussianFactorGraph.run" path="build/gtsam/linear/tests" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments>-j5</buildArguments>
<buildTarget>testGaussianBayesNet.run</buildTarget>
<buildTarget>testGaussianFactorGraphUnordered.run</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
</target>
<target name="testGaussianBayesNetUnordered.run" path="build/gtsam/linear/tests" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments>-j5</buildArguments>
<buildTarget>testGaussianBayesNetUnordered.run</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
@ -2830,10 +2838,10 @@
<useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
</target>
<target name="testSmartProjectionPoseFactor.run" path="build/gtsam/slam/tests" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<target name="testImplicitSchurFactor.run" path="build/gtsam/slam/tests" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments>-j5</buildArguments>
<buildTarget>testSmartProjectionPoseFactor.run</buildTarget>
<buildTarget>testImplicitSchurFactor.run</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>

3
.gitignore vendored
View File

@ -3,4 +3,5 @@
*.pyc
*.DS_Store
/examples/Data/dubrovnik-3-7-pre-rewritten.txt
/examples/Data/pose2example-rewritten.txt
/examples/Data/pose2example-rewritten.txt
/examples/Data/pose3example-rewritten.txt

6
CMakeLists.txt
View File

@ -2,6 +2,12 @@
project(GTSAM CXX C)
cmake_minimum_required(VERSION 2.6)
# new feature to Cmake Version > 2.8.12
# Mac ONLY. Define Relative Path on Mac OS
if(NOT DEFINED CMAKE_MACOSX_RPATH)
set(CMAKE_MACOSX_RPATH 0)
endif()
# Set the version number for the library
set (GTSAM_VERSION_MAJOR 3)
set (GTSAM_VERSION_MINOR 1)

24
cmake/FindMKL.cmake
View File

@ -137,13 +137,16 @@ ELSE() # UNIX and macOS
${MKL_ROOT_DIR}/lib/${MKL_ARCH_DIR}
${MKL_ROOT_DIR}/lib/
)
FIND_LIBRARY(MKL_GNUTHREAD_LIBRARY
mkl_gnu_thread
PATHS
${MKL_ROOT_DIR}/lib/${MKL_ARCH_DIR}
${MKL_ROOT_DIR}/lib/
)
# MKL on Mac OS doesn't ship with GNU thread versions, only Intel versions (see above)
IF(NOT APPLE)
FIND_LIBRARY(MKL_GNUTHREAD_LIBRARY
mkl_gnu_thread
PATHS
${MKL_ROOT_DIR}/lib/${MKL_ARCH_DIR}
${MKL_ROOT_DIR}/lib/
)
ENDIF()
# Intel Libraries
IF("${MKL_ARCH_DIR}" STREQUAL "32")
@ -227,7 +230,12 @@ ELSE() # UNIX and macOS
endforeach()
endforeach()
SET(MKL_LIBRARIES ${MKL_LP_GNUTHREAD_LIBRARIES})
IF(APPLE)
SET(MKL_LIBRARIES ${MKL_LP_INTELTHREAD_LIBRARIES})
ELSE()
SET(MKL_LIBRARIES ${MKL_LP_GNUTHREAD_LIBRARIES})
ENDIF()
MARK_AS_ADVANCED(MKL_CORE_LIBRARY MKL_LP_LIBRARY MKL_ILP_LIBRARY
MKL_SEQUENTIAL_LIBRARY MKL_INTELTHREAD_LIBRARY MKL_GNUTHREAD_LIBRARY)
ENDIF()

6
examples/Data/pose3example-rewritten.txt
View File

@ -1,11 +1,11 @@
VERTEX_SE3:QUAT 0 0 0 0 0 0 0 1
VERTEX_SE3:QUAT 1 1.00137 0.01539 0.004948 0.190253 0.283162 -0.392318 0.85423
VERTEX_SE3:QUAT 2 1.9935 0.023275 0.003793 0.351729 0.597838 -0.584174 -0.421446
VERTEX_SE3:QUAT 2 1.9935 0.023275 0.003793 -0.351729 -0.597838 0.584174 0.421446
VERTEX_SE3:QUAT 3 2.00429 1.02431 0.018047 0.331798 -0.200659 0.919323 0.067024
VERTEX_SE3:QUAT 4 0.999908 1.05507 0.020212 -0.035697 -0.46249 0.445933 0.765488
EDGE_SE3:QUAT 0 1 1.00137 0.01539 0.004948 0.190253 0.283162 -0.392318 0.85423 10000 0 0 0 0 0 10000 0 0 0 0 10000 0 0 0 10000 0 0 10000 0 10000
EDGE_SE3:QUAT 1 2 0.523923 0.776654 0.326659 -0.311512 -0.656877 0.678505 -0.105373 10000 0 0 0 0 0 10000 0 0 0 0 10000 0 0 0 10000 0 0 10000 0 10000
EDGE_SE3:QUAT 1 2 0.523923 0.776654 0.326659 0.311512 0.656877 -0.678505 0.105373 10000 0 0 0 0 0 10000 0 0 0 0 10000 0 0 0 10000 0 0 10000 0 10000
EDGE_SE3:QUAT 2 3 0.910927 0.055169 -0.411761 0.595795 -0.561677 0.079353 0.568551 10000 0 0 0 0 0 10000 0 0 0 0 10000 0 0 0 10000 0 0 10000 0 10000
EDGE_SE3:QUAT 3 4 0.775288 0.228798 -0.596923 -0.592076 0.30338 -0.513225 0.542222 10000 0 0 0 0 0 10000 0 0 0 0 10000 0 0 0 10000 0 0 10000 0 10000
EDGE_SE3:QUAT 3 4 0.775288 0.228798 -0.596923 -0.592077 0.30338 -0.513226 0.542221 10000 0 0 0 0 0 10000 0 0 0 0 10000 0 0 0 10000 0 0 10000 0 10000
EDGE_SE3:QUAT 1 4 -0.577841 0.628016 -0.543592 -0.12525 -0.534379 0.769122 0.327419 10000 0 0 0 0 0 10000 0 0 0 0 10000 0 0 0 10000 0 0 10000 0 10000
EDGE_SE3:QUAT 3 0 -0.623267 0.086928 0.773222 0.104639 0.627755 0.766795 0.083672 10000 0 0 0 0 0 10000 0 0 0 0 10000 0 0 0 10000 0 0 10000 0 10000

11
examples/Data/simpleGraph10gradIter.txt Normal file
View File

@ -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

54
examples/Pose2SLAMExample_g2o.cpp
View File

@ -26,36 +26,72 @@
using namespace std;
using namespace gtsam;
// HOWTO: ./Pose2SLAMExample_g2o inputFile outputFile (maxIterations) (tukey/huber)
int main(const int argc, const char *argv[]) {
// Read graph from file
string g2oFile;
if (argc < 2)
g2oFile = findExampleDataFile("noisyToyGraph.txt");
else
g2oFile = argv[1];
string kernelType = "none";
int maxIterations = 100; // default
string g2oFile = findExampleDataFile("noisyToyGraph.txt"); // default
// Parse user's inputs
if (argc > 1){
g2oFile = argv[1]; // input dataset filename
// outputFile = g2oFile = argv[2]; // done later
}
if (argc > 3){
maxIterations = atoi(argv[3]); // user can specify either tukey or huber
}
if (argc > 4){
kernelType = argv[4]; // user can specify either tukey or huber
}
// reading file and creating factor graph
NonlinearFactorGraph::shared_ptr graph;
Values::shared_ptr initial;
boost::tie(graph, initial) = readG2o(g2oFile);
bool is3D = false;
if(kernelType.compare("none") == 0){
boost::tie(graph, initial) = readG2o(g2oFile,is3D);
}
if(kernelType.compare("huber") == 0){
std::cout << "Using robust kernel: huber " << std::endl;
boost::tie(graph, initial) = readG2o(g2oFile,is3D, KernelFunctionTypeHUBER);
}
if(kernelType.compare("tukey") == 0){
std::cout << "Using robust kernel: tukey " << std::endl;
boost::tie(graph, initial) = readG2o(g2oFile,is3D, KernelFunctionTypeTUKEY);
}
// Add prior on the pose having index (key) = 0
NonlinearFactorGraph graphWithPrior = *graph;
noiseModel::Diagonal::shared_ptr priorModel = //
noiseModel::Diagonal::Variances((Vector(3) << 1e-6, 1e-6, 1e-8));
graphWithPrior.add(PriorFactor<Pose2>(0, Pose2(), priorModel));
std::cout << "Adding prior on pose 0 " << std::endl;
GaussNewtonParams params;
params.setVerbosity("TERMINATION");
if (argc > 3) {
params.maxIterations = maxIterations;
std::cout << "User required to perform maximum " << params.maxIterations << " iterations "<< std::endl;
}
std::cout << "Optimizing the factor graph" << std::endl;
GaussNewtonOptimizer optimizer(graphWithPrior, *initial);
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);
NonlinearFactorGraph::shared_ptr graphNoKernel;
Values::shared_ptr initial2;
boost::tie(graphNoKernel, initial2) = readG2o(g2oFile);
writeG2o(*graphNoKernel, result, outputFile);
std::cout << "done! " << std::endl;
}
return 0;

89
examples/Pose3SLAMExample_changeKeys.cpp Normal file
View File

@ -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;
}

74
examples/Pose3SLAMExample_g2o.cpp Normal file
View File

@ -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;
}

68
examples/Pose3SLAMExample_initializePose3Chordal.cpp Normal file
View File

@ -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;
}

72
examples/Pose3SLAMExample_initializePose3Gradient.cpp Normal file
View File

@ -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;
}

2
gtsam/base/LieMatrix.h
View File

@ -157,7 +157,7 @@ struct LieMatrix : public Matrix {
result.data(), p.rows(), p.cols()) = p;
return result;
}
/// @}
private:

4
gtsam/base/Value.h
View File

@ -36,9 +36,9 @@ namespace gtsam {
* Values can operate generically on Value objects, retracting or computing
* local coordinates for many Value objects of different types.
*
* Inhereting from the DerivedValue class templated provides a generic implementation of
* Inheriting from the DerivedValue class templated provides a generic implementation of
* the pure virtual functions retract_(), localCoordinates_(), and equals_(), eliminating
* the need to implement these functions in your class. Note that you must inheret from
* the need to implement these functions in your class. Note that you must inherit from
* DerivedValue templated on the class you are defining. For example you cannot define
* the following
* \code

146
gtsam/geometry/Cal3DS2.cpp
View File

@ -23,24 +23,9 @@
namespace gtsam {
/* ************************************************************************* */
Cal3DS2::Cal3DS2(const Vector &v):
fx_(v[0]), fy_(v[1]), s_(v[2]), u0_(v[3]), v0_(v[4]), k1_(v[5]), k2_(v[6]), p1_(v[7]), p2_(v[8]){}
/* ************************************************************************* */
Matrix Cal3DS2::K() const {
return (Matrix(3, 3) << fx_, s_, u0_, 0.0, fy_, v0_, 0.0, 0.0, 1.0);
}
/* ************************************************************************* */
Vector Cal3DS2::vector() const {
return (Vector(9) << fx_, fy_, s_, u0_, v0_, k1_, k2_, p1_, p2_);
}
/* ************************************************************************* */
void Cal3DS2::print(const std::string& s_) const {
gtsam::print(K(), s_ + ".K");
gtsam::print(Vector(k()), s_ + ".k");
Base::print(s_);
}
/* ************************************************************************* */
@ -52,135 +37,6 @@ bool Cal3DS2::equals(const Cal3DS2& K, double tol) const {
return true;
}
/* ************************************************************************* */
static Matrix29 D2dcalibration(double x, double y, double xx,
double yy, double xy, double rr, double r4, double pnx, double pny,
const Matrix2& DK) {
Matrix25 DR1;
DR1 << pnx, 0.0, pny, 1.0, 0.0, 0.0, pny, 0.0, 0.0, 1.0;
Matrix24 DR2;
DR2 << x * rr, x * r4, 2 * xy, rr + 2 * xx, //
y * rr, y * r4, rr + 2 * yy, 2 * xy;
Matrix29 D;
D << DR1, DK * DR2;
return D;
}
/* ************************************************************************* */
static Matrix2 D2dintrinsic(double x, double y, double rr,
double g, double k1, double k2, double p1, double p2,
const Matrix2& DK) {
const double drdx = 2. * x;
const double drdy = 2. * y;
const double dgdx = k1 * drdx + k2 * 2. * rr * drdx;
const double dgdy = k1 * drdy + k2 * 2. * rr * drdy;
// Dx = 2*p1*xy + p2*(rr+2*xx);
// Dy = 2*p2*xy + p1*(rr+2*yy);
const double dDxdx = 2. * p1 * y + p2 * (drdx + 4. * x);
const double dDxdy = 2. * p1 * x + p2 * drdy;
const double dDydx = 2. * p2 * y + p1 * drdx;
const double dDydy = 2. * p2 * x + p1 * (drdy + 4. * y);
Matrix2 DR;
DR << g + x * dgdx + dDxdx, x * dgdy + dDxdy, //
y * dgdx + dDydx, g + y * dgdy + dDydy;
return DK * DR;
}
/* ************************************************************************* */
Point2 Cal3DS2::uncalibrate(const Point2& p, boost::optional<Matrix&> H1,
boost::optional<Matrix&> H2) const {
// rr = x^2 + y^2;
// g = (1 + k(1)*rr + k(2)*rr^2);
// dp = [2*k(3)*x*y + k(4)*(rr + 2*x^2); 2*k(4)*x*y + k(3)*(rr + 2*y^2)];
// pi(:,i) = g * pn(:,i) + dp;
const double x = p.x(), y = p.y(), xy = x * y, xx = x * x, yy = y * y;
const double rr = xx + yy;
const double r4 = rr * rr;
const double g = 1. + k1_ * rr + k2_ * r4; // scaling factor
// tangential component
const double dx = 2. * p1_ * xy + p2_ * (rr + 2. * xx);
const double dy = 2. * p2_ * xy + p1_ * (rr + 2. * yy);
// Radial and tangential distortion applied
const double pnx = g * x + dx;
const double pny = g * y + dy;
Matrix2 DK;
if (H1 || H2) DK << fx_, s_, 0.0, fy_;
// Derivative for calibration
if (H1)
*H1 = D2dcalibration(x, y, xx, yy, xy, rr, r4, pnx, pny, DK);
// Derivative for points
if (H2)
*H2 = D2dintrinsic(x, y, rr, g, k1_, k2_, p1_, p2_, DK);
// Regular uncalibrate after distortion
return Point2(fx_ * pnx + s_ * pny + u0_, fy_ * pny + v0_);
}
/* ************************************************************************* */
Point2 Cal3DS2::calibrate(const Point2& pi, const double tol) const {
// Use the following fixed point iteration to invert the radial distortion.
// pn_{t+1} = (inv(K)*pi - dp(pn_{t})) / g(pn_{t})
const Point2 invKPi ((1 / fx_) * (pi.x() - u0_ - (s_ / fy_) * (pi.y() - v0_)),
(1 / fy_) * (pi.y() - v0_));
// initialize by ignoring the distortion at all, might be problematic for pixels around boundary
Point2 pn = invKPi;
// iterate until the uncalibrate is close to the actual pixel coordinate
const int maxIterations = 10;
int iteration;
for (iteration = 0; iteration < maxIterations; ++iteration) {
if (uncalibrate(pn).distance(pi) <= tol) break;
const double x = pn.x(), y = pn.y(), xy = x * y, xx = x * x, yy = y * y;
const double rr = xx + yy;
const double g = (1 + k1_ * rr + k2_ * rr * rr);
const double dx = 2 * p1_ * xy + p2_ * (rr + 2 * xx);
const double dy = 2 * p2_ * xy + p1_ * (rr + 2 * yy);
pn = (invKPi - Point2(dx, dy)) / g;
}
if ( iteration >= maxIterations )
throw std::runtime_error("Cal3DS2::calibrate fails to converge. need a better initialization");
return pn;
}
/* ************************************************************************* */
Matrix Cal3DS2::D2d_intrinsic(const Point2& p) const {
const double x = p.x(), y = p.y(), xx = x * x, yy = y * y;
const double rr = xx + yy;
const double r4 = rr * rr;
const double g = (1 + k1_ * rr + k2_ * r4);
Matrix2 DK;
DK << fx_, s_, 0.0, fy_;
return D2dintrinsic(x, y, rr, g, k1_, k2_, p1_, p2_, DK);
}
/* ************************************************************************* */
Matrix Cal3DS2::D2d_calibration(const Point2& p) const {
const double x = p.x(), y = p.y(), xx = x * x, yy = y * y, xy = x * y;
const double rr = xx + yy;
const double r4 = rr * rr;
const double g = (1 + k1_ * rr + k2_ * r4);
const double dx = 2 * p1_ * xy + p2_ * (rr + 2 * xx);
const double dy = 2 * p2_ * xy + p1_ * (rr + 2 * yy);
const double pnx = g * x + dx;
const double pny = g * y + dy;
Matrix2 DK;
DK << fx_, s_, 0.0, fy_;
return D2dcalibration(x, y, xx, yy, xy, rr, r4, pnx, pny, DK);
}
/* ************************************************************************* */
Cal3DS2 Cal3DS2::retract(const Vector& d) const {
return Cal3DS2(vector() + d);

86
gtsam/geometry/Cal3DS2.h
View File

@ -11,7 +11,7 @@
/**
* @file Cal3DS2.h
* @brief Calibration of a camera with radial distortion
* @brief Calibration of a camera with radial distortion, calculations in base class Cal3DS2_Base
* @date Feb 28, 2010
* @author ydjian
*/
@ -19,7 +19,7 @@
#pragma once
#include <gtsam/base/DerivedValue.h>
#include <gtsam/geometry/Point2.h>
#include <gtsam/geometry/Cal3DS2_Base.h>
namespace gtsam {
@ -37,29 +37,21 @@ namespace gtsam {
* k3 (rr + 2 Pn.y^2) + 2*k4 pn.x pn.y ]
* pi = K*pn
*/
class GTSAM_EXPORT Cal3DS2 {
class GTSAM_EXPORT Cal3DS2 : public Cal3DS2_Base, public DerivedValue<Cal3DS2> {
protected:
double fx_, fy_, s_, u0_, v0_ ; // focal length, skew and principal point
double k1_, k2_ ; // radial 2nd-order and 4th-order
double p1_, p2_ ; // tangential distortion
typedef Cal3DS2_Base Base;
public:
Matrix K() const ;
Eigen::Vector4d k() const { return Eigen::Vector4d(k1_, k2_, p1_, p2_); }
Vector vector() const ;
/// @name Standard Constructors
/// @{
/// Default Constructor with only unit focal length
Cal3DS2() : fx_(1), fy_(1), s_(0), u0_(0), v0_(0), k1_(0), k2_(0), p1_(0), p2_(0) {}
Cal3DS2() : Base() {}
Cal3DS2(double fx, double fy, double s, double u0, double v0,
double k1, double k2, double p1 = 0.0, double p2 = 0.0) :
fx_(fx), fy_(fy), s_(s), u0_(u0), v0_(v0), k1_(k1), k2_(k2), p1_(p1), p2_(p2) {}
Base(fx, fy, s, u0, v0, k1, k2, p1, p2) {}
virtual ~Cal3DS2() {}
@ -67,7 +59,7 @@ public:
/// @name Advanced Constructors
/// @{
Cal3DS2(const Vector &v) ;
Cal3DS2(const Vector &v) : Base(v) {}
/// @}
/// @name Testable
@ -79,57 +71,6 @@ public:
/// assert equality up to a tolerance
bool equals(const Cal3DS2& K, double tol = 10e-9) const;
/// @}
/// @name Standard Interface
/// @{
/// focal length x
inline double fx() const { return fx_;}
/// focal length x
inline double fy() const { return fy_;}
/// skew
inline double skew() const { return s_;}
/// image center in x
inline double px() const { return u0_;}
/// image center in y
inline double py() const { return v0_;}
/// First distortion coefficient
inline double k1() const { return k1_;}
/// Second distortion coefficient
inline double k2() const { return k2_;}
/// First tangential distortion coefficient
inline double p1() const { return p1_;}
/// Second tangential distortion coefficient
inline double p2() const { return p2_;}
/**
* convert intrinsic coordinates xy to (distorted) image coordinates uv
* @param p point in intrinsic coordinates
* @param Dcal optional 2*9 Jacobian wrpt Cal3DS2 parameters
* @param Dp optional 2*2 Jacobian wrpt intrinsic coordinates
* @return point in (distorted) image coordinates
*/
Point2 uncalibrate(const Point2& p,
boost::optional<Matrix&> Dcal = boost::none,
boost::optional<Matrix&> Dp = boost::none) const ;
/// Convert (distorted) image coordinates uv to intrinsic coordinates xy
Point2 calibrate(const Point2& p, const double tol=1e-5) const;
/// Derivative of uncalibrate wrpt intrinsic coordinates
Matrix D2d_intrinsic(const Point2& p) const ;
/// Derivative of uncalibrate wrpt the calibration parameters
Matrix D2d_calibration(const Point2& p) const ;
/// @}
/// @name Manifold
/// @{
@ -155,15 +96,10 @@ private:
template<class Archive>
void serialize(Archive & ar, const unsigned int version)
{
ar & BOOST_SERIALIZATION_NVP(fx_);
ar & BOOST_SERIALIZATION_NVP(fy_);
ar & BOOST_SERIALIZATION_NVP(s_);
ar & BOOST_SERIALIZATION_NVP(u0_);
ar & BOOST_SERIALIZATION_NVP(v0_);
ar & BOOST_SERIALIZATION_NVP(k1_);
ar & BOOST_SERIALIZATION_NVP(k2_);
ar & BOOST_SERIALIZATION_NVP(p1_);
ar & BOOST_SERIALIZATION_NVP(p2_);
ar & boost::serialization::make_nvp("Cal3DS2",
boost::serialization::base_object<Value>(*this));
ar & boost::serialization::make_nvp("Cal3DS2",
boost::serialization::base_object<Cal3DS2_Base>(*this));
}
};

187
gtsam/geometry/Cal3DS2_Base.cpp Normal file
View File

@ -0,0 +1,187 @@
/* ----------------------------------------------------------------------------
* 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 Cal3DS2_Base.cpp
* @date Feb 28, 2010
* @author ydjian
*/
#include <gtsam/base/Vector.h>
#include <gtsam/base/Matrix.h>
#include <gtsam/geometry/Point2.h>
#include <gtsam/geometry/Point3.h>
#include <gtsam/geometry/Cal3DS2_Base.h>
namespace gtsam {
/* ************************************************************************* */
Cal3DS2_Base::Cal3DS2_Base(const Vector &v):
fx_(v[0]), fy_(v[1]), s_(v[2]), u0_(v[3]), v0_(v[4]), k1_(v[5]), k2_(v[6]), p1_(v[7]), p2_(v[8]){}
/* ************************************************************************* */
Matrix Cal3DS2_Base::K() const {
return (Matrix(3, 3) << fx_, s_, u0_, 0.0, fy_, v0_, 0.0, 0.0, 1.0);
}
/* ************************************************************************* */
Vector Cal3DS2_Base::vector() const {
return (Vector(9) << fx_, fy_, s_, u0_, v0_, k1_, k2_, p1_, p2_);
}
/* ************************************************************************* */
void Cal3DS2_Base::print(const std::string& s_) const {
gtsam::print(K(), s_ + ".K");
gtsam::print(Vector(k()), s_ + ".k");
}
/* ************************************************************************* */
bool Cal3DS2_Base::equals(const Cal3DS2_Base& K, double tol) const {
if (fabs(fx_ - K.fx_) > tol || fabs(fy_ - K.fy_) > tol || fabs(s_ - K.s_) > tol ||
fabs(u0_ - K.u0_) > tol || fabs(v0_ - K.v0_) > tol || fabs(k1_ - K.k1_) > tol ||
fabs(k2_ - K.k2_) > tol || fabs(p1_ - K.p1_) > tol || fabs(p2_ - K.p2_) > tol)
return false;
return true;
}
/* ************************************************************************* */
static Eigen::Matrix<double, 2, 9> D2dcalibration(double x, double y, double xx,
double yy, double xy, double rr, double r4, double pnx, double pny,
const Eigen::Matrix<double, 2, 2>& DK) {
Eigen::Matrix<double, 2, 5> DR1;
DR1 << pnx, 0.0, pny, 1.0, 0.0, 0.0, pny, 0.0, 0.0, 1.0;
Eigen::Matrix<double, 2, 4> DR2;
DR2 << x * rr, x * r4, 2 * xy, rr + 2 * xx, //
y * rr, y * r4, rr + 2 * yy, 2 * xy;
Eigen::Matrix<double, 2, 9> D;
D << DR1, DK * DR2;
return D;
}
/* ************************************************************************* */
static Eigen::Matrix<double, 2, 2> D2dintrinsic(double x, double y, double rr,
double g, double k1, double k2, double p1, double p2,
const Eigen::Matrix<double, 2, 2>& DK) {
const double drdx = 2. * x;
const double drdy = 2. * y;
const double dgdx = k1 * drdx + k2 * 2. * rr * drdx;
const double dgdy = k1 * drdy + k2 * 2. * rr * drdy;
// Dx = 2*p1*xy + p2*(rr+2*xx);
// Dy = 2*p2*xy + p1*(rr+2*yy);
const double dDxdx = 2. * p1 * y + p2 * (drdx + 4. * x);
const double dDxdy = 2. * p1 * x + p2 * drdy;
const double dDydx = 2. * p2 * y + p1 * drdx;
const double dDydy = 2. * p2 * x + p1 * (drdy + 4. * y);
Eigen::Matrix<double, 2, 2> DR;
DR << g + x * dgdx + dDxdx, x * dgdy + dDxdy, //
y * dgdx + dDydx, g + y * dgdy + dDydy;
return DK * DR;
}
/* ************************************************************************* */
Point2 Cal3DS2_Base::uncalibrate(const Point2& p, boost::optional<Matrix&> H1,
boost::optional<Matrix&> H2) const {
// rr = x^2 + y^2;
// g = (1 + k(1)*rr + k(2)*rr^2);
// dp = [2*k(3)*x*y + k(4)*(rr + 2*x^2); 2*k(4)*x*y + k(3)*(rr + 2*y^2)];
// pi(:,i) = g * pn(:,i) + dp;
const double x = p.x(), y = p.y(), xy = x * y, xx = x * x, yy = y * y;
const double rr = xx + yy;
const double r4 = rr * rr;
const double g = 1. + k1_ * rr + k2_ * r4; // scaling factor
// tangential component
const double dx = 2. * p1_ * xy + p2_ * (rr + 2. * xx);
const double dy = 2. * p2_ * xy + p1_ * (rr + 2. * yy);
// Radial and tangential distortion applied
const double pnx = g * x + dx;
const double pny = g * y + dy;
Eigen::Matrix<double, 2, 2> DK;
if (H1 || H2) DK << fx_, s_, 0.0, fy_;
// Derivative for calibration
if (H1)
*H1 = D2dcalibration(x, y, xx, yy, xy, rr, r4, pnx, pny, DK);
// Derivative for points
if (H2)
*H2 = D2dintrinsic(x, y, rr, g, k1_, k2_, p1_, p2_, DK);
// Regular uncalibrate after distortion
return Point2(fx_ * pnx + s_ * pny + u0_, fy_ * pny + v0_);
}
/* ************************************************************************* */
Point2 Cal3DS2_Base::calibrate(const Point2& pi, const double tol) const {
// Use the following fixed point iteration to invert the radial distortion.
// pn_{t+1} = (inv(K)*pi - dp(pn_{t})) / g(pn_{t})
const Point2 invKPi ((1 / fx_) * (pi.x() - u0_ - (s_ / fy_) * (pi.y() - v0_)),
(1 / fy_) * (pi.y() - v0_));
// initialize by ignoring the distortion at all, might be problematic for pixels around boundary
Point2 pn = invKPi;
// iterate until the uncalibrate is close to the actual pixel coordinate
const int maxIterations = 10;
int iteration;
for (iteration = 0; iteration < maxIterations; ++iteration) {
if (uncalibrate(pn).distance(pi) <= tol) break;
const double x = pn.x(), y = pn.y(), xy = x * y, xx = x * x, yy = y * y;
const double rr = xx + yy;
const double g = (1 + k1_ * rr + k2_ * rr * rr);
const double dx = 2 * p1_ * xy + p2_ * (rr + 2 * xx);
const double dy = 2 * p2_ * xy + p1_ * (rr + 2 * yy);
pn = (invKPi - Point2(dx, dy)) / g;
}
if ( iteration >= maxIterations )
throw std::runtime_error("Cal3DS2::calibrate fails to converge. need a better initialization");
return pn;
}
/* ************************************************************************* */
Matrix Cal3DS2_Base::D2d_intrinsic(const Point2& p) const {
const double x = p.x(), y = p.y(), xx = x * x, yy = y * y;
const double rr = xx + yy;
const double r4 = rr * rr;
const double g = (1 + k1_ * rr + k2_ * r4);
Eigen::Matrix<double, 2, 2> DK;
DK << fx_, s_, 0.0, fy_;
return D2dintrinsic(x, y, rr, g, k1_, k2_, p1_, p2_, DK);
}
/* ************************************************************************* */
Matrix Cal3DS2_Base::D2d_calibration(const Point2& p) const {
const double x = p.x(), y = p.y(), xx = x * x, yy = y * y, xy = x * y;
const double rr = xx + yy;
const double r4 = rr * rr;
const double g = (1 + k1_ * rr + k2_ * r4);
const double dx = 2 * p1_ * xy + p2_ * (rr + 2 * xx);
const double dy = 2 * p2_ * xy + p1_ * (rr + 2 * yy);
const double pnx = g * x + dx;
const double pny = g * y + dy;
Eigen::Matrix<double, 2, 2> DK;
DK << fx_, s_, 0.0, fy_;
return D2dcalibration(x, y, xx, yy, xy, rr, r4, pnx, pny, DK);
}
}
/* ************************************************************************* */

158
gtsam/geometry/Cal3DS2_Base.h Normal file
View File

@ -0,0 +1,158 @@
/* ----------------------------------------------------------------------------
* 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 Cal3DS2.h
* @brief Calibration of a camera with radial distortion
* @date Feb 28, 2010
* @author ydjian
*/
#pragma once
#include <gtsam/base/DerivedValue.h>
#include <gtsam/geometry/Point2.h>
namespace gtsam {
/**
* @brief Calibration of a camera with radial distortion
* @addtogroup geometry
* \nosubgrouping
*
* Uses same distortionmodel as OpenCV, with
* http://docs.opencv.org/modules/calib3d/doc/camera_calibration_and_3d_reconstruction.html
* but using only k1,k2,p1, and p2 coefficients.
* K = [ fx s u0 ; 0 fy v0 ; 0 0 1 ]
* rr = Pn.x^2 + Pn.y^2
* \hat{pn} = (1 + k1*rr + k2*rr^2 ) pn + [ 2*k3 pn.x pn.y + k4 (rr + 2 Pn.x^2) ;
* k3 (rr + 2 Pn.y^2) + 2*k4 pn.x pn.y ]
* pi = K*pn
*/
class GTSAM_EXPORT Cal3DS2_Base {
protected:
double fx_, fy_, s_, u0_, v0_ ; // focal length, skew and principal point
double k1_, k2_ ; // radial 2nd-order and 4th-order
double p1_, p2_ ; // tangential distortion
public:
Matrix K() const ;
Eigen::Vector4d k() const { return Eigen::Vector4d(k1_, k2_, p1_, p2_); }
Vector vector() const ;
/// @name Standard Constructors
/// @{
/// Default Constructor with only unit focal length
Cal3DS2_Base() : fx_(1), fy_(1), s_(0), u0_(0), v0_(0), k1_(0), k2_(0), p1_(0), p2_(0) {}
Cal3DS2_Base(double fx, double fy, double s, double u0, double v0,
double k1, double k2, double p1 = 0.0, double p2 = 0.0) :
fx_(fx), fy_(fy), s_(s), u0_(u0), v0_(v0), k1_(k1), k2_(k2), p1_(p1), p2_(p2) {}
/// @}
/// @name Advanced Constructors
/// @{
Cal3DS2_Base(const Vector &v) ;
/// @}
/// @name Testable
/// @{
/// print with optional string
void print(const std::string& s = "") const ;
/// assert equality up to a tolerance
bool equals(const Cal3DS2_Base& K, double tol = 10e-9) const;
/// @}
/// @name Standard Interface
/// @{
/// focal length x
inline double fx() const { return fx_;}
/// focal length x
inline double fy() const { return fy_;}
/// skew
inline double skew() const { return s_;}
/// image center in x
inline double px() const { return u0_;}
/// image center in y
inline double py() const { return v0_;}
/// First distortion coefficient
inline double k1() const { return k1_;}
/// Second distortion coefficient
inline double k2() const { return k2_;}
/// First tangential distortion coefficient
inline double p1() const { return p1_;}
/// Second tangential distortion coefficient
inline double p2() const { return p2_;}
/**
* convert intrinsic coordinates xy to (distorted) image coordinates uv
* @param p point in intrinsic coordinates
* @param Dcal optional 2*9 Jacobian wrpt Cal3DS2 parameters
* @param Dp optional 2*2 Jacobian wrpt intrinsic coordinates
* @return point in (distorted) image coordinates
*/
Point2 uncalibrate(const Point2& p,
boost::optional<Matrix&> Dcal = boost::none,
boost::optional<Matrix&> Dp = boost::none) const ;
/// Convert (distorted) image coordinates uv to intrinsic coordinates xy
Point2 calibrate(const Point2& p, const double tol=1e-5) const;
/// Derivative of uncalibrate wrpt intrinsic coordinates
Matrix D2d_intrinsic(const Point2& p) const ;
/// Derivative of uncalibrate wrpt the calibration parameters
Matrix D2d_calibration(const Point2& p) const ;
private:
/// @}
/// @name Advanced Interface
/// @{
/** Serialization function */
friend class boost::serialization::access;
template<class Archive>
void serialize(Archive & ar, const unsigned int version)
{
ar & BOOST_SERIALIZATION_NVP(fx_);
ar & BOOST_SERIALIZATION_NVP(fy_);
ar & BOOST_SERIALIZATION_NVP(s_);
ar & BOOST_SERIALIZATION_NVP(u0_);
ar & BOOST_SERIALIZATION_NVP(v0_);
ar & BOOST_SERIALIZATION_NVP(k1_);
ar & BOOST_SERIALIZATION_NVP(k2_);
ar & BOOST_SERIALIZATION_NVP(p1_);
ar & BOOST_SERIALIZATION_NVP(p2_);
}
/// @}
};
}

18
gtsam/geometry/Cal3Unified.h
View File

@ -22,8 +22,8 @@
#pragma once
#include <gtsam/geometry/Cal3DS2.h>
#include <gtsam/geometry/Point2.h>
#include <gtsam/geometry/Cal3DS2_Base.h>
#include <gtsam/base/DerivedValue.h>
namespace gtsam {
@ -40,20 +40,18 @@ namespace gtsam {
* k3 (rr + 2 Pn.y^2) + 2*k4 pn.x pn.y ]
* pi = K*pn
*/
class GTSAM_EXPORT Cal3Unified : public Cal3DS2 {
class GTSAM_EXPORT Cal3Unified : public Cal3DS2_Base, public DerivedValue<Cal3Unified> {
typedef Cal3Unified This;
typedef Cal3DS2 Base;
typedef Cal3DS2_Base Base;
private:
double xi_; // mirror parameter
public:
/// dimension of the variable - used to autodetect sizes
static const size_t dimension = 10;
Vector vector() const ;
Vector vector() const ;
/// @name Standard Constructors
/// @{
@ -91,7 +89,7 @@ public:
/**
* convert intrinsic coordinates xy to image coordinates uv
* @param p point in intrinsic coordinates
* @param Dcal optional 2*9 Jacobian wrpt Cal3DS2 parameters
* @param Dcal optional 2*10 Jacobian wrpt Cal3Unified parameters
* @param Dp optional 2*2 Jacobian wrpt intrinsic coordinates
* @return point in image coordinates
*/
@ -131,6 +129,10 @@ private:
template<class Archive>
void serialize(Archive & ar, const unsigned int version)
{
ar & boost::serialization::make_nvp("Cal3Unified",
boost::serialization::base_object<Value>(*this));
ar & boost::serialization::make_nvp("Cal3Unified",
boost::serialization::base_object<Cal3DS2_Base>(*this));
ar & BOOST_SERIALIZATION_NVP(xi_);
}

21
gtsam/geometry/Unit3.cpp
View File

@ -21,7 +21,16 @@
#include <gtsam/geometry/Unit3.h>
#include <gtsam/geometry/Point2.h>
#include <boost/random/mersenne_twister.hpp>
#ifdef __clang__
# pragma clang diagnostic push
# pragma clang diagnostic ignored "-Wunused-variable"
#endif
#include <boost/random/uniform_on_sphere.hpp>
#ifdef __clang__
# pragma clang diagnostic pop
#endif
#include <boost/random/variate_generator.hpp>
#include <iostream>
@ -58,11 +67,11 @@ Unit3 Unit3::Random(boost::mt19937 & rng) {
}
/* ************************************************************************* */
const Matrix& Unit3::basis() const {
const Unit3::Matrix32& Unit3::basis() const {
// Return cached version if exists
if (B_.rows() == 3)
return B_;
if (B_)
return *B_;
// Get the axis of rotation with the minimum projected length of the point
Point3 axis;
@ -83,9 +92,9 @@ const Matrix& Unit3::basis() const {
b2 = b2 / b2.norm();
// Create the basis matrix
B_ = Matrix(3, 2);
B_ << b1.x(), b2.x(), b1.y(), b2.y(), b1.z(), b2.z();
return B_;
B_.reset(Unit3::Matrix32());
(*B_) << b1.x(), b2.x(), b1.y(), b2.y(), b1.z(), b2.z();
return *B_;
}
/* ************************************************************************* */

7
gtsam/geometry/Unit3.h
View File

@ -23,6 +23,7 @@
#include <gtsam/geometry/Point3.h>
#include <gtsam/base/DerivedValue.h>
#include <boost/random/mersenne_twister.hpp>
#include <boost/optional.hpp>
namespace gtsam {
@ -31,8 +32,10 @@ class GTSAM_EXPORT Unit3{
private:
typedef Eigen::Matrix<double,3,2> Matrix32;
Point3 p_; ///< The location of the point on the unit sphere
mutable Matrix B_; ///< Cached basis
mutable boost::optional<Matrix32> B_; ///< Cached basis
public:
@ -84,7 +87,7 @@ public:
* It is a 3*2 matrix [b1 b2] composed of two orthogonal directions
* tangent to the sphere at the current direction.
*/
const Matrix& basis() const;
const Matrix32& basis() const;
/// Return skew-symmetric associated with 3D point on unit sphere
Matrix skew() const;

16
gtsam/geometry/tests/testCal3Unified.cpp
View File

@ -19,6 +19,9 @@
#include <gtsam/base/numericalDerivative.h>
#include <gtsam/geometry/Cal3Unified.h>
#include <gtsam/nonlinear/Values.h>
#include <gtsam/inference/Key.h>
using namespace gtsam;
GTSAM_CONCEPT_TESTABLE_INST(Cal3Unified)
@ -97,6 +100,19 @@ TEST( Cal3Unified, retract)
CHECK(assert_equal(d,K.localCoordinates(actual),1e-9));
}
/* ************************************************************************* */
TEST( Cal3Unified, DerivedValue)
{
Values values;
Cal3Unified cal(1, 2, 3, 4, 5, 6, 7, 8, 9, 10);
Key key = 1;
values.insert(key, cal);
Cal3Unified calafter = values.at<Cal3Unified>(key);
CHECK(assert_equal(cal,calafter,1e-9));
}
/* ************************************************************************* */
int main() { TestResult tr; return TestRegistry::runAllTests(tr); }
/* ************************************************************************* */

43
gtsam/geometry/tests/testPinholeCamera.cpp
View File

@ -183,25 +183,30 @@ TEST( PinholeCamera, Dproject)
}
/* ************************************************************************* */
//static Point2 projectInfinity3(const Pose3& pose, const Point2& point2D, const Cal3_S2& cal) {
// Point3 point(point2D.x(), point2D.y(), 1.0);
// return Camera(pose,cal).projectPointAtInfinity(point);
//}
//
//TEST( PinholeCamera, Dproject_Infinity)
//{
// Matrix Dpose, Dpoint, Dcal;
// Point2 point2D(-0.08,-0.08);
// Point3 point3D(point1.x(), point1.y(), 1.0);
// Point2 result = camera.projectPointAtInfinity(point3D, Dpose, Dpoint, Dcal);
// Matrix numerical_pose = numericalDerivative31(projectInfinity3, pose1, point2D, K);
// Matrix numerical_point = numericalDerivative32(projectInfinity3, pose1, point2D, K);
// Matrix numerical_cal = numericalDerivative33(projectInfinity3, pose1, point2D, K);
// CHECK(assert_equal(numerical_pose, Dpose, 1e-7));
// CHECK(assert_equal(numerical_point, Dpoint, 1e-7));
// CHECK(assert_equal(numerical_cal, Dcal, 1e-7));
//}
//
static Point2 projectInfinity3(const Pose3& pose, const Point3& point3D, const Cal3_S2& cal) {
return Camera(pose,cal).projectPointAtInfinity(point3D);
}
TEST( PinholeCamera, Dproject_Infinity)
{
Matrix Dpose, Dpoint, Dcal;
Point3 point3D(point1.x(), point1.y(), -10.0); // a point in front of the camera
// test Projection
Point2 actual = camera.projectPointAtInfinity(point3D, Dpose, Dpoint, Dcal);
Point2 expected(-5.0, 5.0);
CHECK(assert_equal(actual, expected, 1e-7));
// test Jacobians
Matrix numerical_pose = numericalDerivative31(projectInfinity3, pose1, point3D, K);
Matrix numerical_point = numericalDerivative32(projectInfinity3, pose1, point3D, K);
Matrix numerical_point2x2 = numerical_point.block(0,0,2,2); // only the direction to the point matters
Matrix numerical_cal = numericalDerivative33(projectInfinity3, pose1, point3D, K);
CHECK(assert_equal(numerical_pose, Dpose, 1e-7));
CHECK(assert_equal(numerical_point2x2, Dpoint, 1e-7));
CHECK(assert_equal(numerical_cal, Dcal, 1e-7));
}
/* ************************************************************************* */
static Point2 project4(const Camera& camera, const Point3& point) {
return camera.project2(point);

8
gtsam/geometry/tests/testRot3.cpp
View File

@ -184,7 +184,15 @@ TEST(Rot3, log)
CHECK_OMEGA( PI, 0, 0)
CHECK_OMEGA( 0, PI, 0)
CHECK_OMEGA( 0, 0, PI)
// Windows and Linux have flipped sign in quaternion mode
#if !defined(__APPLE__) && defined (GTSAM_USE_QUATERNIONS)
w = (Vector(3) << x*PI, y*PI, z*PI);
R = Rot3::rodriguez(w);
EXPECT(assert_equal(Vector(-w), Rot3::Logmap(R),1e-12));
#else
CHECK_OMEGA(x*PI,y*PI,z*PI)
#endif
// Check 360 degree rotations
#define CHECK_OMEGA_ZERO(X,Y,Z) \

1
gtsam/geometry/tests/testRot3M.cpp
View File

@ -37,7 +37,6 @@ GTSAM_CONCEPT_LIE_INST(Rot3)
static Rot3 R = Rot3::rodriguez(0.1, 0.4, 0.2);
static Point3 P(0.2, 0.7, -2.0);
static double error = 1e-9, epsilon = 0.001;
static const Matrix I3 = eye(3);
/* ************************************************************************* */

3
gtsam/geometry/tests/testSerializationGeometry.cpp
View File

@ -25,6 +25,7 @@
#include <gtsam/geometry/PinholeCamera.h>
#include <gtsam/geometry/Cal3DS2.h>
#include <gtsam/geometry/Cal3Bundler.h>
#include <gtsam/geometry/Cal3Unified.h>
#include <gtsam/geometry/StereoCamera.h>
#include <gtsam/geometry/StereoPoint2.h>
@ -46,6 +47,7 @@ static Cal3Bundler cal3(1.0, 2.0, 3.0);
static Cal3_S2Stereo cal4(1.0, 2.0, 3.0, 4.0, 5.0, 6.0);
static Cal3_S2Stereo::shared_ptr cal4ptr(new Cal3_S2Stereo(cal4));
static CalibratedCamera cal5(Pose3(rt3, pt3));
static Cal3Unified cal6(1.0, 2.0, 3.0, 4.0, 5.0, 6.0, 7.0, 8.0, 9.0, 10.0);
static PinholeCamera<Cal3_S2> cam1(pose3, cal1);
static StereoCamera cam2(pose3, cal4ptr);
@ -66,6 +68,7 @@ TEST (Serialization, text_geometry) {
EXPECT(equalsObj(cal3));
EXPECT(equalsObj(cal4));
EXPECT(equalsObj(cal5));
EXPECT(equalsObj(cal6));
EXPECT(equalsObj(cam1));
EXPECT(equalsObj(cam2));

4
gtsam/geometry/triangulation.h
View File

@ -174,7 +174,7 @@ Point3 triangulateNonlinear(
* @param poses A vector of camera poses
* @param sharedCal shared pointer to single calibration object
* @param measurements A vector of camera measurements
* @param rank tolerance, default 1e-9
* @param rank_tol rank tolerance, default 1e-9
* @param optimize Flag to turn on nonlinear refinement of triangulation
* @return Returns a Point3
*/
@ -222,7 +222,7 @@ Point3 triangulatePoint3(const std::vector<Pose3>& poses,
* no other checks to verify the quality of the triangulation.
* @param cameras pinhole cameras
* @param measurements A vector of camera measurements
* @param rank tolerance, default 1e-9
* @param rank_tol rank tolerance, default 1e-9
* @param optimize Flag to turn on nonlinear refinement of triangulation
* @return Returns a Point3
*/

2
gtsam/linear/GaussianConditional.h
View File

@ -111,7 +111,7 @@ namespace gtsam {
* assumed to have already been solved in and their values are read from \c x.
* This function works for multiple frontal variables.
*
* Given the Gaussian conditional with log likelihood \f$ |R x_f - (d - S x_s)|^2,
* Given the Gaussian conditional with log likelihood \f$ |R x_f - (d - S x_s)|^2 \f$,
* where \f$ f \f$ are the frontal variables and \f$ s \f$ are the separator
* variables of this conditional, this solve function computes
* \f$ x_f = R^{-1} (d - S x_s) \f$ using back-substitution.

6
gtsam/linear/GaussianFactorGraph.cpp
View File

@ -85,7 +85,7 @@ namespace gtsam {
dims_accumulated.resize(dims.size()+1,0);
dims_accumulated[0]=0;
for (size_t i=1; i<dims_accumulated.size(); i++)
dims_accumulated[i] = dims_accumulated[i-1]+dims[i-1];
dims_accumulated[i] = dims_accumulated[i-1]+dims[i-1];
return dims_accumulated;
}
@ -358,8 +358,8 @@ namespace gtsam {
/* ************************************************************************* */
void GaussianFactorGraph::multiplyHessianAdd(double alpha,
const double* x, double* y) const {
vector<size_t> FactorKeys = getkeydim();
BOOST_FOREACH(const GaussianFactor::shared_ptr& f, *this)
vector<size_t> FactorKeys = getkeydim();
BOOST_FOREACH(const GaussianFactor::shared_ptr& f, *this)
f->multiplyHessianAdd(alpha, x, y, FactorKeys);
}

2
gtsam/linear/HessianFactor.cpp
View File

@ -538,7 +538,7 @@ void HessianFactor::multiplyHessianAdd(double alpha, const VectorValues& x,
// copy to yvalues
for(DenseIndex i = 0; i < (DenseIndex)size(); ++i) {
bool didNotExist;
bool didNotExist;
VectorValues::iterator it;
boost::tie(it, didNotExist) = yvalues.tryInsert(keys_[i], Vector());
if (didNotExist)

96
gtsam/navigation/CombinedImuFactor.h
View File

@ -344,12 +344,12 @@ namespace gtsam {
/** Constructor */
CombinedImuFactor(
Key pose_i, ///< previous pose key
Key vel_i, ///< previous velocity key
Key pose_j, ///< current pose key
Key vel_j, ///< current velocity key
Key bias_i, ///< previous bias key
Key bias_j, ///< current bias key
Key pose_i, ///< previous pose key
Key vel_i, ///< previous velocity key
Key pose_j, ///< current pose key
Key vel_j, ///< current velocity key
Key bias_i, ///< previous bias key
Key bias_j, ///< current bias key
const CombinedPreintegratedMeasurements& preintegratedMeasurements, ///< Preintegrated IMU measurements
const Vector3& gravity, ///< gravity vector
const Vector3& omegaCoriolis, ///< rotation rate of inertial frame
@ -479,33 +479,33 @@ namespace gtsam {
Matrix3 dfPdPi;
Matrix3 dfVdPi;
if(use2ndOrderCoriolis_){
dfPdPi = - Rot_i.matrix() + 0.5 * skewSymmetric(omegaCoriolis_) * skewSymmetric(omegaCoriolis_) * Rot_i.matrix() * deltaTij*deltaTij;
dfVdPi = skewSymmetric(omegaCoriolis_) * skewSymmetric(omegaCoriolis_) * Rot_i.matrix() * deltaTij;
dfPdPi = - Rot_i.matrix() + 0.5 * skewSymmetric(omegaCoriolis_) * skewSymmetric(omegaCoriolis_) * Rot_i.matrix() * deltaTij*deltaTij;
dfVdPi = skewSymmetric(omegaCoriolis_) * skewSymmetric(omegaCoriolis_) * Rot_i.matrix() * deltaTij;
}
else{
dfPdPi = - Rot_i.matrix();
dfVdPi = Matrix3::Zero();
dfPdPi = - Rot_i.matrix();
dfVdPi = Matrix3::Zero();
}
(*H1) <<
// dfP/dRi
Rot_i.matrix() * skewSymmetric(preintegratedMeasurements_.deltaPij
+ preintegratedMeasurements_.delPdelBiasOmega * biasOmegaIncr + preintegratedMeasurements_.delPdelBiasAcc * biasAccIncr),
// dfP/dPi
dfPdPi,
// dfV/dRi
Rot_i.matrix() * skewSymmetric(preintegratedMeasurements_.deltaVij
+ preintegratedMeasurements_.delVdelBiasOmega * biasOmegaIncr + preintegratedMeasurements_.delVdelBiasAcc * biasAccIncr),
// dfV/dPi
dfVdPi,
// dfR/dRi
Jrinv_fRhat * (- Rot_j.between(Rot_i).matrix() - fRhat.inverse().matrix() * Jtheta),
// dfR/dPi
Matrix3::Zero(),
//dBiasAcc/dPi
Matrix3::Zero(), Matrix3::Zero(),
//dBiasOmega/dPi
Matrix3::Zero(), Matrix3::Zero();
(*H1) <<
// dfP/dRi
Rot_i.matrix() * skewSymmetric(preintegratedMeasurements_.deltaPij
+ preintegratedMeasurements_.delPdelBiasOmega * biasOmegaIncr + preintegratedMeasurements_.delPdelBiasAcc * biasAccIncr),
// dfP/dPi
dfPdPi,
// dfV/dRi
Rot_i.matrix() * skewSymmetric(preintegratedMeasurements_.deltaVij
+ preintegratedMeasurements_.delVdelBiasOmega * biasOmegaIncr + preintegratedMeasurements_.delVdelBiasAcc * biasAccIncr),
// dfV/dPi
dfVdPi,
// dfR/dRi
Jrinv_fRhat * (- Rot_j.between(Rot_i).matrix() - fRhat.inverse().matrix() * Jtheta),
// dfR/dPi
Matrix3::Zero(),
//dBiasAcc/dPi
Matrix3::Zero(), Matrix3::Zero(),
//dBiasOmega/dPi
Matrix3::Zero(), Matrix3::Zero();
}
if(H2) {
@ -516,13 +516,13 @@ namespace gtsam {
+ skewSymmetric(omegaCoriolis_) * deltaTij * deltaTij, // Coriolis term - we got rid of the 2 wrt ins paper
// dfV/dVi
- Matrix3::Identity()
+ 2 * skewSymmetric(omegaCoriolis_) * deltaTij, // Coriolis term
// dfR/dVi
Matrix3::Zero(),
//dBiasAcc/dVi
Matrix3::Zero(),
//dBiasOmega/dVi
Matrix3::Zero();
+ 2 * skewSymmetric(omegaCoriolis_) * deltaTij, // Coriolis term
// dfR/dVi
Matrix3::Zero(),
//dBiasAcc/dVi
Matrix3::Zero(),
//dBiasOmega/dVi
Matrix3::Zero();
}
if(H3) {
@ -642,21 +642,21 @@ namespace gtsam {
// Predict state at time j
/* ---------------------------------------------------------------------------------------------------- */
Vector3 pos_j = pos_i + Rot_i.matrix() * (preintegratedMeasurements.deltaPij
+ preintegratedMeasurements.delPdelBiasAcc * biasAccIncr
+ preintegratedMeasurements.delPdelBiasOmega * biasOmegaIncr)
+ vel_i * deltaTij
- skewSymmetric(omegaCoriolis) * vel_i * deltaTij*deltaTij // Coriolis term - we got rid of the 2 wrt ins paper
+ 0.5 * gravity * deltaTij*deltaTij;
+ preintegratedMeasurements.delPdelBiasAcc * biasAccIncr
+ preintegratedMeasurements.delPdelBiasOmega * biasOmegaIncr)
+ vel_i * deltaTij
- skewSymmetric(omegaCoriolis) * vel_i * deltaTij*deltaTij // Coriolis term - we got rid of the 2 wrt ins paper
+ 0.5 * gravity * deltaTij*deltaTij;
vel_j = Vector3(vel_i + Rot_i.matrix() * (preintegratedMeasurements.deltaVij
+ preintegratedMeasurements.delVdelBiasAcc * biasAccIncr
+ preintegratedMeasurements.delVdelBiasOmega * biasOmegaIncr)
- 2 * skewSymmetric(omegaCoriolis) * vel_i * deltaTij // Coriolis term
+ gravity * deltaTij);
vel_j = Vector3(vel_i + Rot_i.matrix() * (preintegratedMeasurements.deltaVij
+ preintegratedMeasurements.delVdelBiasAcc * biasAccIncr
+ preintegratedMeasurements.delVdelBiasOmega * biasOmegaIncr)
- 2 * skewSymmetric(omegaCoriolis) * vel_i * deltaTij // Coriolis term
+ gravity * deltaTij);
if(use2ndOrderCoriolis){
pos_j += - 0.5 * skewSymmetric(omegaCoriolis) * skewSymmetric(omegaCoriolis) * pos_i * deltaTij*deltaTij; // 2nd order coriolis term for position
vel_j += - skewSymmetric(omegaCoriolis) * skewSymmetric(omegaCoriolis) * pos_i * deltaTij; // 2nd order term for velocity
pos_j += - 0.5 * skewSymmetric(omegaCoriolis) * skewSymmetric(omegaCoriolis) * pos_i * deltaTij*deltaTij; // 2nd order coriolis term for position
vel_j += - skewSymmetric(omegaCoriolis) * skewSymmetric(omegaCoriolis) * pos_i * deltaTij; // 2nd order term for velocity
}
const Rot3 deltaRij_biascorrected = preintegratedMeasurements.deltaRij.retract(preintegratedMeasurements.delRdelBiasOmega * biasOmegaIncr, Rot3::EXPMAP);

74
gtsam/navigation/ImuFactor.h
View File

@ -307,11 +307,11 @@ namespace gtsam {
/** Constructor */
ImuFactor(
Key pose_i, ///< previous pose key
Key vel_i, ///< previous velocity key
Key pose_j, ///< current pose key
Key vel_j, ///< current velocity key
Key bias, ///< previous bias key
Key pose_i, ///< previous pose key
Key vel_i, ///< previous velocity key
Key pose_j, ///< current pose key
Key vel_j, ///< current velocity key
Key bias, ///< previous bias key
const PreintegratedMeasurements& preintegratedMeasurements, ///< preintegrated IMU measurements
const Vector3& gravity, ///< gravity vector
const Vector3& omegaCoriolis, ///< rotation rate of the inertial frame
@ -418,29 +418,29 @@ namespace gtsam {
Matrix3 dfPdPi;
Matrix3 dfVdPi;
if(use2ndOrderCoriolis_){
dfPdPi = - Rot_i.matrix() + 0.5 * skewSymmetric(omegaCoriolis_) * skewSymmetric(omegaCoriolis_) * Rot_i.matrix() * deltaTij*deltaTij;
dfVdPi = skewSymmetric(omegaCoriolis_) * skewSymmetric(omegaCoriolis_) * Rot_i.matrix() * deltaTij;
dfPdPi = - Rot_i.matrix() + 0.5 * skewSymmetric(omegaCoriolis_) * skewSymmetric(omegaCoriolis_) * Rot_i.matrix() * deltaTij*deltaTij;
dfVdPi = skewSymmetric(omegaCoriolis_) * skewSymmetric(omegaCoriolis_) * Rot_i.matrix() * deltaTij;
}
else{
dfPdPi = - Rot_i.matrix();
dfVdPi = Matrix3::Zero();
dfPdPi = - Rot_i.matrix();
dfVdPi = Matrix3::Zero();
}
(*H1) <<
// dfP/dRi
Rot_i.matrix() * skewSymmetric(preintegratedMeasurements_.deltaPij
+ preintegratedMeasurements_.delPdelBiasOmega * biasOmegaIncr + preintegratedMeasurements_.delPdelBiasAcc * biasAccIncr),
// dfP/dPi
dfPdPi,
// dfV/dRi
Rot_i.matrix() * skewSymmetric(preintegratedMeasurements_.deltaVij
+ preintegratedMeasurements_.delVdelBiasOmega * biasOmegaIncr + preintegratedMeasurements_.delVdelBiasAcc * biasAccIncr),
// dfV/dPi
dfVdPi,
// dfR/dRi
Jrinv_fRhat * (- Rot_j.between(Rot_i).matrix() - fRhat.inverse().matrix() * Jtheta),
// dfR/dPi
Matrix3::Zero();
(*H1) <<
// dfP/dRi
Rot_i.matrix() * skewSymmetric(preintegratedMeasurements_.deltaPij
+ preintegratedMeasurements_.delPdelBiasOmega * biasOmegaIncr + preintegratedMeasurements_.delPdelBiasAcc * biasAccIncr),
// dfP/dPi
dfPdPi,
// dfV/dRi
Rot_i.matrix() * skewSymmetric(preintegratedMeasurements_.deltaVij
+ preintegratedMeasurements_.delVdelBiasOmega * biasOmegaIncr + preintegratedMeasurements_.delVdelBiasAcc * biasAccIncr),
// dfV/dPi
dfVdPi,
// dfR/dRi
Jrinv_fRhat * (- Rot_j.between(Rot_i).matrix() - fRhat.inverse().matrix() * Jtheta),
// dfR/dPi
Matrix3::Zero();
}
if(H2) {
@ -539,22 +539,22 @@ namespace gtsam {
// Predict state at time j
/* ---------------------------------------------------------------------------------------------------- */
Vector3 pos_j = pos_i + Rot_i.matrix() * (preintegratedMeasurements.deltaPij
+ preintegratedMeasurements.delPdelBiasAcc * biasAccIncr
+ preintegratedMeasurements.delPdelBiasOmega * biasOmegaIncr)
+ vel_i * deltaTij
- skewSymmetric(omegaCoriolis) * vel_i * deltaTij*deltaTij // Coriolis term - we got rid of the 2 wrt ins paper
+ 0.5 * gravity * deltaTij*deltaTij;
Vector3 pos_j = pos_i + Rot_i.matrix() * (preintegratedMeasurements.deltaPij
+ preintegratedMeasurements.delPdelBiasAcc * biasAccIncr
+ preintegratedMeasurements.delPdelBiasOmega * biasOmegaIncr)
+ vel_i * deltaTij
- skewSymmetric(omegaCoriolis) * vel_i * deltaTij*deltaTij // Coriolis term - we got rid of the 2 wrt ins paper
+ 0.5 * gravity * deltaTij*deltaTij;
vel_j = Vector3(vel_i + Rot_i.matrix() * (preintegratedMeasurements.deltaVij
+ preintegratedMeasurements.delVdelBiasAcc * biasAccIncr
+ preintegratedMeasurements.delVdelBiasOmega * biasOmegaIncr)
- 2 * skewSymmetric(omegaCoriolis) * vel_i * deltaTij // Coriolis term
+ gravity * deltaTij);
vel_j = Vector3(vel_i + Rot_i.matrix() * (preintegratedMeasurements.deltaVij
+ preintegratedMeasurements.delVdelBiasAcc * biasAccIncr
+ preintegratedMeasurements.delVdelBiasOmega * biasOmegaIncr)
- 2 * skewSymmetric(omegaCoriolis) * vel_i * deltaTij // Coriolis term
+ gravity * deltaTij);
if(use2ndOrderCoriolis){
pos_j += - 0.5 * skewSymmetric(omegaCoriolis) * skewSymmetric(omegaCoriolis) * pos_i * deltaTij*deltaTij; // 2nd order coriolis term for position
vel_j += - skewSymmetric(omegaCoriolis) * skewSymmetric(omegaCoriolis) * pos_i * deltaTij; // 2nd order term for velocity
pos_j += - 0.5 * skewSymmetric(omegaCoriolis) * skewSymmetric(omegaCoriolis) * pos_i * deltaTij*deltaTij; // 2nd order coriolis term for position
vel_j += - skewSymmetric(omegaCoriolis) * skewSymmetric(omegaCoriolis) * pos_i * deltaTij; // 2nd order term for velocity
}
const Rot3 deltaRij_biascorrected = preintegratedMeasurements.deltaRij.retract(preintegratedMeasurements.delRdelBiasOmega * biasOmegaIncr, Rot3::EXPMAP);

64
gtsam/nonlinear/NonlinearFactorGraph.cpp
View File

@ -176,11 +176,11 @@ void NonlinearFactorGraph::saveGraph(std::ostream &stm, const Values& values,
stm << "];\n";
if (firstTimePoses) {
lastKey = key;
firstTimePoses = false;
lastKey = key;
firstTimePoses = false;
} else {
stm << " var" << key << "--" << "var" << lastKey << ";\n";
lastKey = key;
stm << " var" << key << "--" << "var" << lastKey << ";\n";
lastKey = key;
}
}
stm << "\n";
@ -219,37 +219,37 @@ void NonlinearFactorGraph::saveGraph(std::ostream &stm, const Values& values,
// Create factors and variable connections
for(size_t i = 0; i < this->size(); ++i) {
if(graphvizFormatting.plotFactorPoints){
// Make each factor a dot
stm << " factor" << i << "[label=\"\", shape=point";
{
map<size_t, Point2>::const_iterator pos = graphvizFormatting.factorPositions.find(i);
if(pos != graphvizFormatting.factorPositions.end())
stm << ", pos=\"" << graphvizFormatting.scale*(pos->second.x() - minX) << "," << graphvizFormatting.scale*(pos->second.y() - minY) << "!\"";
}
stm << "];\n";
// Make each factor a dot
stm << " factor" << i << "[label=\"\", shape=point";
{
map<size_t, Point2>::const_iterator pos = graphvizFormatting.factorPositions.find(i);
if(pos != graphvizFormatting.factorPositions.end())
stm << ", pos=\"" << graphvizFormatting.scale*(pos->second.x() - minX) << "," << graphvizFormatting.scale*(pos->second.y() - minY) << "!\"";
}
stm << "];\n";
// Make factor-variable connections
if(graphvizFormatting.connectKeysToFactor && this->at(i)) {
BOOST_FOREACH(Key key, *this->at(i)) {
stm << " var" << key << "--" << "factor" << i << ";\n";
}
}
// Make factor-variable connections
if(graphvizFormatting.connectKeysToFactor && this->at(i)) {
BOOST_FOREACH(Key key, *this->at(i)) {
stm << " var" << key << "--" << "factor" << i << ";\n";
}
}
}
}
else {
if(this->at(i)) {
Key k;
bool firstTime = true;
BOOST_FOREACH(Key key, *this->at(i)) {
if(firstTime){
k = key;
firstTime = false;
continue;
}
stm << " var" << key << "--" << "var" << k << ";\n";
k = key;
}
}
if(this->at(i)) {
Key k;
bool firstTime = true;
BOOST_FOREACH(Key key, *this->at(i)) {
if(firstTime){
k = key;
firstTime = false;
continue;
}
stm << " var" << key << "--" << "var" << k << ";\n";
k = key;
}
}
}
}
}

8
gtsam/nonlinear/NonlinearOptimizer.cpp
View File

@ -77,9 +77,11 @@ void NonlinearOptimizer::defaultOptimize() {
params.errorTol, currentError, this->error(), params.verbosity));
// Printing if verbose
if (params.verbosity >= NonlinearOptimizerParams::TERMINATION &&
this->iterations() >= params.maxIterations)
cout << "Terminating because reached maximum iterations" << endl;
if (params.verbosity >= NonlinearOptimizerParams::TERMINATION) {
cout << "iterations: " << this->iterations() << " >? " << params.maxIterations << endl;
if (this->iterations() >= params.maxIterations)
cout << "Terminating because reached maximum iterations" << endl;
}
}
/* ************************************************************************* */

10
gtsam/slam/EssentialMatrixFactor.h
View File

@ -28,6 +28,7 @@ public:
/**
* Constructor
* @param key Essential Matrix variable key
* @param pA point in first camera, in calibrated coordinates
* @param pB point in second camera, in calibrated coordinates
* @param model noise model is about dot product in ideal, homogeneous coordinates
@ -41,6 +42,7 @@ public:
/**
* Constructor
* @param key Essential Matrix variable key
* @param pA point in first camera, in pixel coordinates
* @param pB point in second camera, in pixel coordinates
* @param model noise model is about dot product in ideal, homogeneous coordinates
@ -97,6 +99,8 @@ public:
/**
* Constructor
* @param key1 Essential Matrix variable key
* @param key2 Inverse depth variable key
* @param pA point in first camera, in calibrated coordinates
* @param pB point in second camera, in calibrated coordinates
* @param model noise model should be in pixels, as well
@ -111,6 +115,8 @@ public:
/**
* Constructor
* @param key1 Essential Matrix variable key
* @param key2 Inverse depth variable key
* @param pA point in first camera, in pixel coordinates
* @param pB point in second camera, in pixel coordinates
* @param K calibration object, will be used only in constructor
@ -216,6 +222,8 @@ public:
/**
* Constructor
* @param key1 Essential Matrix variable key
* @param key2 Inverse depth variable key
* @param pA point in first camera, in calibrated coordinates
* @param pB point in second camera, in calibrated coordinates
* @param bRc extra rotation between "body" and "camera" frame
@ -228,6 +236,8 @@ public:
/**
* Constructor
* @param key1 Essential Matrix variable key
* @param key2 Inverse depth variable key
* @param pA point in first camera, in pixel coordinates
* @param pB point in second camera, in pixel coordinates
* @param K calibration object, will be used only in constructor

43
gtsam/slam/ImplicitSchurFactor.h
View File

@ -80,7 +80,7 @@ public:
}
/// Get matrix P
inline const Matrix& getPointCovariance() const {
inline const Matrix3& getPointCovariance() const {
return PointCovariance_;
}
@ -285,26 +285,27 @@ public:
return 0.5 * (result + f);
}
/// needed to be GaussianFactor - (I - E*P*E')*(F*x - b)
// This is wrong and does not match the definition in Hessian
// virtual double error(const VectorValues& x) const {
//
// // resize does not do malloc if correct size
// e1.resize(size());
// e2.resize(size());
//
// // e1 = F * x - b = (2m*dm)*dm
// for (size_t k = 0; k < size(); ++k)
// e1[k] = Fblocks_[k].second * x.at(keys_[k]) - b_.segment < 2 > (k * 2);
// projectError(e1, e2);
//
// double result = 0;
// for (size_t k = 0; k < size(); ++k)
// result += dot(e2[k], e2[k]);
//
// std::cout << "implicitFactor::error result " << result << std::endl;
// return 0.5 * result;
// }
// needed to be GaussianFactor - (I - E*P*E')*(F*x - b)
// This is wrong and does not match the definition in Hessian,
// but it matches the definition of the Jacobian factor (JF)
double errorJF(const VectorValues& x) const {
// resize does not do malloc if correct size
e1.resize(size());
e2.resize(size());
// e1 = F * x - b = (2m*dm)*dm
for (size_t k = 0; k < size(); ++k)
e1[k] = Fblocks_[k].second * x.at(keys_[k]) - b_.segment < 2 > (k * 2);
projectError(e1, e2);
double result = 0;
for (size_t k = 0; k < size(); ++k)
result += dot(e2[k], e2[k]);
// std::cout << "implicitFactor::error result " << result << std::endl;
return 0.5 * result;
}
/**
* @brief Calculate corrected error Q*e = (I - E*P*E')*e
*/

410
gtsam/slam/InitializePose3.cpp Normal file
View File

@ -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

59
gtsam/slam/InitializePose3.h Normal file
View File

@ -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

2
gtsam/slam/PoseRotationPrior.h
View File

@ -77,7 +77,7 @@ public:
(*H).middleCols(rotInterval.first, rDim).setIdentity(rDim, rDim);
}
return Rotation::Logmap(newR) - Rotation::Logmap(measured_);
return measured_.localCoordinates(newR);
}
private:

23
gtsam/slam/dataset.cpp
View File

@ -101,6 +101,25 @@ static SharedNoiseModel readNoiseModel(ifstream& is, bool smart,
double v1, v2, v3, v4, v5, v6;
is >> v1 >> v2 >> v3 >> v4 >> v5 >> v6;
if (noiseFormat == NoiseFormatAUTO)
{
// Try to guess covariance matrix layout
if(v1 != 0.0 && v2 == 0.0 && v3 != 0.0 && v4 != 0.0 && v5 == 0.0 && v6 == 0.0)
{
// NoiseFormatGRAPH
noiseFormat = NoiseFormatGRAPH;
}
else if(v1 != 0.0 && v2 == 0.0 && v3 == 0.0 && v4 != 0.0 && v5 == 0.0 && v6 != 0.0)
{
// NoiseFormatCOV
noiseFormat = NoiseFormatCOV;
}
else
{
throw std::invalid_argument("load2D: unrecognized covariance matrix format in dataset file. Please specify the noise format.");
}
}
// Read matrix and check that diagonal entries are non-zero
Matrix M(3, 3);
switch (noiseFormat) {
@ -162,7 +181,7 @@ static SharedNoiseModel readNoiseModel(ifstream& is, bool smart,
}
/* ************************************************************************* */
GraphAndValues load2D(const string& filename, SharedNoiseModel model, int maxID,
GraphAndValues load2D(const string& filename, SharedNoiseModel model, Key maxID,
bool addNoise, bool smart, NoiseFormat noiseFormat,
KernelFunctionType kernelFunctionType) {
@ -210,7 +229,7 @@ GraphAndValues load2D(const string& filename, SharedNoiseModel model, int maxID,
}
// Parse the pose constraints
int id1, id2;
Key id1, id2;
bool haveLandmark = false;
while (!is.eof()) {
if (!(is >> tag))

9
gtsam/slam/dataset.h
View File

@ -57,7 +57,8 @@ enum NoiseFormat {
NoiseFormatG2O, ///< Information matrix I11, I12, I13, I22, I23, I33
NoiseFormatTORO, ///< Information matrix, but inf_ff inf_fs inf_ss inf_rr inf_fr inf_sr
NoiseFormatGRAPH, ///< default: toro-style order, but covariance matrix !
NoiseFormatCOV ///< Covariance matrix C11, C12, C13, C22, C23, C33
NoiseFormatCOV, ///< Covariance matrix C11, C12, C13, C22, C23, C33
NoiseFormatAUTO ///< Try to guess covariance matrix layout
};
/// Robust kernel type to wrap around quadratic noise model
@ -79,7 +80,7 @@ GTSAM_EXPORT GraphAndValues load2D(
std::pair<std::string, SharedNoiseModel> dataset, int maxID = 0,
bool addNoise = false,
bool smart = true, //
NoiseFormat noiseFormat = NoiseFormatGRAPH,
NoiseFormat noiseFormat = NoiseFormatAUTO,
KernelFunctionType kernelFunctionType = KernelFunctionTypeNONE);
/**
@ -94,8 +95,8 @@ GTSAM_EXPORT GraphAndValues load2D(
* @return graph and initial values
*/
GTSAM_EXPORT GraphAndValues load2D(const std::string& filename,
SharedNoiseModel model = SharedNoiseModel(), int maxID = 0, bool addNoise =
false, bool smart = true, NoiseFormat noiseFormat = NoiseFormatGRAPH, //
SharedNoiseModel model = SharedNoiseModel(), Key maxID = 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

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

@ -56,6 +56,17 @@ TEST( dataSet, load2D)
EXPECT(assert_equal(expected, *actual));
}
/* ************************************************************************* */
TEST( dataSet, load2DVictoriaPark)
{
const string filename = findExampleDataFile("victoria_park.txt");
NonlinearFactorGraph::shared_ptr graph;
Values::shared_ptr initial;
boost::tie(graph, initial) = load2D(filename);
EXPECT_LONGS_EQUAL(10608,graph->size());
EXPECT_LONGS_EQUAL(7120,initial->size());
}
/* ************************************************************************* */
TEST( dataSet, Balbianello)
{

259
gtsam/slam/tests/testImplicitSchurFactor.cpp Normal file
View File

@ -0,0 +1,259 @@
/**
* @file testImplicitSchurFactor.cpp
* @brief unit test implicit jacobian factors
* @author Frank Dellaert
* @date Oct 20, 2013
*/
//#include <gtsam_unstable/slam/ImplicitSchurFactor.h>
#include <gtsam/slam/ImplicitSchurFactor.h>
//#include <gtsam_unstable/slam/JacobianFactorQ.h>
#include <gtsam/slam/JacobianFactorQ.h>
//#include "gtsam_unstable/slam/JacobianFactorQR.h"
#include "gtsam/slam/JacobianFactorQR.h"
#include <gtsam/base/timing.h>
#include <gtsam/linear/VectorValues.h>
#include <gtsam/linear/NoiseModel.h>
#include <gtsam/linear/GaussianFactor.h>
#include <boost/assign/list_of.hpp>
#include <boost/assign/std/vector.hpp>
#include <boost/range/iterator_range.hpp>
#include <boost/range/adaptor/map.hpp>
#include <CppUnitLite/TestHarness.h>
using namespace std;
using namespace boost::assign;
using namespace gtsam;
// F
typedef Eigen::Matrix<double, 2, 6> Matrix26;
const Matrix26 F0 = Matrix26::Ones();
const Matrix26 F1 = 2 * Matrix26::Ones();
const Matrix26 F3 = 3 * Matrix26::Ones();
const vector<pair<Key, Matrix26> > Fblocks = list_of<pair<Key, Matrix> > //
(make_pair(0, F0))(make_pair(1, F1))(make_pair(3, F3));
// RHS and sigmas
const Vector b = (Vector(6) << 1., 2., 3., 4., 5., 6.);
//*************************************************************************************
TEST( implicitSchurFactor, creation ) {
// Matrix E = Matrix::Ones(6,3);
Matrix E = zeros(6, 3);
E.block<2,2>(0, 0) = eye(2);
E.block<2,3>(2, 0) = 2 * ones(2, 3);
Matrix3 P = (E.transpose() * E).inverse();
ImplicitSchurFactor<6> expected(Fblocks, E, P, b);
Matrix expectedP = expected.getPointCovariance();
EXPECT(assert_equal(expectedP, P));
}
/* ************************************************************************* */
TEST( implicitSchurFactor, addHessianMultiply ) {
Matrix E = zeros(6, 3);
E.block<2,2>(0, 0) = eye(2);
E.block<2,3>(2, 0) = 2 * ones(2, 3);
E.block<2,2>(4, 1) = eye(2);
Matrix3 P = (E.transpose() * E).inverse();
double alpha = 0.5;
VectorValues xvalues = map_list_of //
(0, gtsam::repeat(6, 2))//
(1, gtsam::repeat(6, 4))//
(2, gtsam::repeat(6, 0))// distractor
(3, gtsam::repeat(6, 8));
VectorValues yExpected = map_list_of//
(0, gtsam::repeat(6, 27))//
(1, gtsam::repeat(6, -40))//
(2, gtsam::repeat(6, 0))// distractor
(3, gtsam::repeat(6, 279));
// Create full F
size_t M=4, m = 3, d = 6;
Matrix F(2 * m, d * M);
F << F0, zeros(2, d * 3), zeros(2, d), F1, zeros(2, d*2), zeros(2, d * 3), F3;
// Calculate expected result F'*alpha*(I - E*P*E')*F*x
FastVector<Key> keys;
keys += 0,1,2,3;
Vector x = xvalues.vector(keys);
Vector expected = zero(24);
ImplicitSchurFactor<6>::multiplyHessianAdd(F, E, P, alpha, x, expected);
EXPECT(assert_equal(expected, yExpected.vector(keys), 1e-8));
// Create ImplicitSchurFactor
ImplicitSchurFactor<6> implicitFactor(Fblocks, E, P, b);
VectorValues zero = 0 * yExpected;// quick way to get zero w right structure
{ // First Version
VectorValues yActual = zero;
implicitFactor.multiplyHessianAdd(alpha, xvalues, yActual);
EXPECT(assert_equal(yExpected, yActual, 1e-8));
implicitFactor.multiplyHessianAdd(alpha, xvalues, yActual);
EXPECT(assert_equal(2 * yExpected, yActual, 1e-8));
implicitFactor.multiplyHessianAdd(-1, xvalues, yActual);
EXPECT(assert_equal(zero, yActual, 1e-8));
}
typedef Eigen::Matrix<double, 24, 1> DeltaX;
typedef Eigen::Map<DeltaX> XMap;
double* y = new double[24];
double* xdata = x.data();
{ // Raw memory Version
std::fill(y, y + 24, 0);// zero y !
implicitFactor.multiplyHessianAdd(alpha, xdata, y);
EXPECT(assert_equal(expected, XMap(y), 1e-8));
implicitFactor.multiplyHessianAdd(alpha, xdata, y);
EXPECT(assert_equal(Vector(2 * expected), XMap(y), 1e-8));
implicitFactor.multiplyHessianAdd(-1, xdata, y);
EXPECT(assert_equal(Vector(0 * expected), XMap(y), 1e-8));
}
// Create JacobianFactor with same error
const SharedDiagonal model;
JacobianFactorQ<6> jf(Fblocks, E, P, b, model);
{ // error
double expectedError = jf.error(xvalues);
double actualError = implicitFactor.errorJF(xvalues);
DOUBLES_EQUAL(expectedError,actualError,1e-7)
}
{ // JacobianFactor with same error
VectorValues yActual = zero;
jf.multiplyHessianAdd(alpha, xvalues, yActual);
EXPECT(assert_equal(yExpected, yActual, 1e-8));
jf.multiplyHessianAdd(alpha, xvalues, yActual);
EXPECT(assert_equal(2 * yExpected, yActual, 1e-8));
jf.multiplyHessianAdd(-1, xvalues, yActual);
EXPECT(assert_equal(zero, yActual, 1e-8));
}
{ // check hessian Diagonal
VectorValues diagExpected = jf.hessianDiagonal();
VectorValues diagActual = implicitFactor.hessianDiagonal();
EXPECT(assert_equal(diagExpected, diagActual, 1e-8));
}
{ // check hessian Block Diagonal
map<Key,Matrix> BD = jf.hessianBlockDiagonal();
map<Key,Matrix> actualBD = implicitFactor.hessianBlockDiagonal();
LONGS_EQUAL(3,actualBD.size());
EXPECT(assert_equal(BD[0],actualBD[0]));
EXPECT(assert_equal(BD[1],actualBD[1]));
EXPECT(assert_equal(BD[3],actualBD[3]));
}
{ // Raw memory Version
std::fill(y, y + 24, 0);// zero y !
jf.multiplyHessianAdd(alpha, xdata, y);
EXPECT(assert_equal(expected, XMap(y), 1e-8));
jf.multiplyHessianAdd(alpha, xdata, y);
EXPECT(assert_equal(Vector(2 * expected), XMap(y), 1e-8));
jf.multiplyHessianAdd(-1, xdata, y);
EXPECT(assert_equal(Vector(0 * expected), XMap(y), 1e-8));
}
{ // Check gradientAtZero
VectorValues expected = jf.gradientAtZero();
VectorValues actual = implicitFactor.gradientAtZero();
EXPECT(assert_equal(expected, actual, 1e-8));
}
// Create JacobianFactorQR
JacobianFactorQR<6> jfq(Fblocks, E, P, b, model);
{
const SharedDiagonal model;
VectorValues yActual = zero;
jfq.multiplyHessianAdd(alpha, xvalues, yActual);
EXPECT(assert_equal(yExpected, yActual, 1e-8));
jfq.multiplyHessianAdd(alpha, xvalues, yActual);
EXPECT(assert_equal(2 * yExpected, yActual, 1e-8));
jfq.multiplyHessianAdd(-1, xvalues, yActual);
EXPECT(assert_equal(zero, yActual, 1e-8));
}
{ // Raw memory Version
std::fill(y, y + 24, 0);// zero y !
jfq.multiplyHessianAdd(alpha, xdata, y);
EXPECT(assert_equal(expected, XMap(y), 1e-8));
jfq.multiplyHessianAdd(alpha, xdata, y);
EXPECT(assert_equal(Vector(2 * expected), XMap(y), 1e-8));
jfq.multiplyHessianAdd(-1, xdata, y);
EXPECT(assert_equal(Vector(0 * expected), XMap(y), 1e-8));
}
delete [] y;
}
/* ************************************************************************* */
TEST(implicitSchurFactor, hessianDiagonal)
{
/* TESTED AGAINST MATLAB
* F = [ones(2,6) zeros(2,6) zeros(2,6)
zeros(2,6) 2*ones(2,6) zeros(2,6)
zeros(2,6) zeros(2,6) 3*ones(2,6)]
E = [[1:6] [1:6] [0.5 1:5]];
E = reshape(E',3,6)'
P = inv(E' * E)
H = F' * (eye(6) - E * P * E') * F
diag(H)
*/
Matrix E(6,3);
E.block<2,3>(0, 0) << 1,2,3,4,5,6;
E.block<2,3>(2, 0) << 1,2,3,4,5,6;
E.block<2,3>(4, 0) << 0.5,1,2,3,4,5;
Matrix3 P = (E.transpose() * E).inverse();
ImplicitSchurFactor<6> factor(Fblocks, E, P, b);
// hessianDiagonal
VectorValues expected;
expected.insert(0, 1.195652*ones(6));
expected.insert(1, 4.782608*ones(6));
expected.insert(3, 7.043478*ones(6));
EXPECT(assert_equal(expected, factor.hessianDiagonal(),1e-5));
// hessianBlockDiagonal
map<Key,Matrix> actualBD = factor.hessianBlockDiagonal();
LONGS_EQUAL(3,actualBD.size());
Matrix FtE0 = F0.transpose() * E.block<2,3>(0, 0);
Matrix FtE1 = F1.transpose() * E.block<2,3>(2, 0);
Matrix FtE3 = F3.transpose() * E.block<2,3>(4, 0);
// variant one
EXPECT(assert_equal(F0.transpose()*F0-FtE0*P*FtE0.transpose(),actualBD[0]));
EXPECT(assert_equal(F1.transpose()*F1-FtE1*P*FtE1.transpose(),actualBD[1]));
EXPECT(assert_equal(F3.transpose()*F3-FtE3*P*FtE3.transpose(),actualBD[3]));
// variant two
Matrix I2 = eye(2);
Matrix E0 = E.block<2,3>(0, 0);
Matrix F0t = F0.transpose();
EXPECT(assert_equal(F0t*F0-F0t*E0*P*E0.transpose()*F0,actualBD[0]));
EXPECT(assert_equal(F0t*(F0-E0*P*E0.transpose()*F0),actualBD[0]));
Matrix M1 = F0t*(F0-E0*P*E0.transpose()*F0);
Matrix M2 = F0t*F0-F0t*E0*P*E0.transpose()*F0;
EXPECT(assert_equal( M1 , actualBD[0] ));
EXPECT(assert_equal( M1 , M2 ));
Matrix M1b = F0t*(E0*P*E0.transpose()*F0);
Matrix M2b = F0t*E0*P*E0.transpose()*F0;
EXPECT(assert_equal( M1b , M2b ));
EXPECT(assert_equal(F0t*(I2-E0*P*E0.transpose())*F0,actualBD[0]));
EXPECT(assert_equal(F1.transpose()*F1-FtE1*P*FtE1.transpose(),actualBD[1]));
EXPECT(assert_equal(F3.transpose()*F3-FtE3*P*FtE3.transpose(),actualBD[3]));
}
/* ************************************************************************* */
int main(void) {
TestResult tr;
int result = TestRegistry::runAllTests(tr);
return result;
}
//*************************************************************************************

259
gtsam/slam/tests/testInitializePose3.cpp Normal file
View File

@ -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);
}
/* ************************************************************************* */

56
gtsam/slam/tests/testLago.cpp
View File

@ -36,7 +36,7 @@ using namespace boost::assign;
static Symbol x0('x', 0), x1('x', 1), x2('x', 2), x3('x', 3);
static SharedNoiseModel model(noiseModel::Isotropic::Sigma(3, 0.1));
namespace simple {
namespace simpleLago {
// We consider a small graph:
// symbolic FG
// x2 0 1
@ -67,7 +67,7 @@ NonlinearFactorGraph graph() {
/* *************************************************************************** */
TEST( Lago, checkSTandChords ) {
NonlinearFactorGraph g = simple::graph();
NonlinearFactorGraph g = simpleLago::graph();
PredecessorMap<Key> tree = findMinimumSpanningTree<NonlinearFactorGraph, Key,
BetweenFactor<Pose2> >(g);
@ -84,7 +84,7 @@ TEST( Lago, checkSTandChords ) {
/* *************************************************************************** */
TEST( Lago, orientationsOverSpanningTree ) {
NonlinearFactorGraph g = simple::graph();
NonlinearFactorGraph g = simpleLago::graph();
PredecessorMap<Key> tree = findMinimumSpanningTree<NonlinearFactorGraph, Key,
BetweenFactor<Pose2> >(g);
@ -115,7 +115,7 @@ TEST( Lago, orientationsOverSpanningTree ) {
/* *************************************************************************** */
TEST( Lago, regularizedMeasurements ) {
NonlinearFactorGraph g = simple::graph();
NonlinearFactorGraph g = simpleLago::graph();
PredecessorMap<Key> tree = findMinimumSpanningTree<NonlinearFactorGraph, Key,
BetweenFactor<Pose2> >(g);
@ -141,7 +141,7 @@ TEST( Lago, regularizedMeasurements ) {
/* *************************************************************************** */
TEST( Lago, smallGraphVectorValues ) {
bool useOdometricPath = false;
VectorValues initial = lago::initializeOrientations(simple::graph(), useOdometricPath);
VectorValues initial = lago::initializeOrientations(simpleLago::graph(), useOdometricPath);
// comparison is up to M_PI, that's why we add some multiples of 2*M_PI
EXPECT(assert_equal((Vector(1) << 0.0), initial.at(x0), 1e-6));
@ -153,7 +153,7 @@ TEST( Lago, smallGraphVectorValues ) {
/* *************************************************************************** */
TEST( Lago, smallGraphVectorValuesSP ) {
VectorValues initial = lago::initializeOrientations(simple::graph());
VectorValues initial = lago::initializeOrientations(simpleLago::graph());
// comparison is up to M_PI, that's why we add some multiples of 2*M_PI
EXPECT(assert_equal((Vector(1) << 0.0), initial.at(x0), 1e-6));
@ -165,8 +165,8 @@ TEST( Lago, smallGraphVectorValuesSP ) {
/* *************************************************************************** */
TEST( Lago, multiplePosePriors ) {
bool useOdometricPath = false;
NonlinearFactorGraph g = simple::graph();
g.add(PriorFactor<Pose2>(x1, simple::pose1, model));
NonlinearFactorGraph g = simpleLago::graph();
g.add(PriorFactor<Pose2>(x1, simpleLago::pose1, model));
VectorValues initial = lago::initializeOrientations(g, useOdometricPath);
// comparison is up to M_PI, that's why we add some multiples of 2*M_PI
@ -178,8 +178,8 @@ TEST( Lago, multiplePosePriors ) {
/* *************************************************************************** */
TEST( Lago, multiplePosePriorsSP ) {
NonlinearFactorGraph g = simple::graph();
g.add(PriorFactor<Pose2>(x1, simple::pose1, model));
NonlinearFactorGraph g = simpleLago::graph();
g.add(PriorFactor<Pose2>(x1, simpleLago::pose1, model));
VectorValues initial = lago::initializeOrientations(g);
// comparison is up to M_PI, that's why we add some multiples of 2*M_PI
@ -192,8 +192,8 @@ TEST( Lago, multiplePosePriorsSP ) {
/* *************************************************************************** */
TEST( Lago, multiplePoseAndRotPriors ) {
bool useOdometricPath = false;
NonlinearFactorGraph g = simple::graph();
g.add(PriorFactor<Rot2>(x1, simple::pose1.theta(), model));
NonlinearFactorGraph g = simpleLago::graph();
g.add(PriorFactor<Rot2>(x1, simpleLago::pose1.theta(), model));
VectorValues initial = lago::initializeOrientations(g, useOdometricPath);
// comparison is up to M_PI, that's why we add some multiples of 2*M_PI
@ -205,8 +205,8 @@ TEST( Lago, multiplePoseAndRotPriors ) {
/* *************************************************************************** */
TEST( Lago, multiplePoseAndRotPriorsSP ) {
NonlinearFactorGraph g = simple::graph();
g.add(PriorFactor<Rot2>(x1, simple::pose1.theta(), model));
NonlinearFactorGraph g = simpleLago::graph();
g.add(PriorFactor<Rot2>(x1, simpleLago::pose1.theta(), model));
VectorValues initial = lago::initializeOrientations(g);
// comparison is up to M_PI, that's why we add some multiples of 2*M_PI
@ -221,20 +221,20 @@ TEST( Lago, smallGraphValues ) {
// we set the orientations in the initial guess to zero
Values initialGuess;
initialGuess.insert(x0,Pose2(simple::pose0.x(),simple::pose0.y(),0.0));
initialGuess.insert(x1,Pose2(simple::pose1.x(),simple::pose1.y(),0.0));
initialGuess.insert(x2,Pose2(simple::pose2.x(),simple::pose2.y(),0.0));
initialGuess.insert(x3,Pose2(simple::pose3.x(),simple::pose3.y(),0.0));
initialGuess.insert(x0,Pose2(simpleLago::pose0.x(),simpleLago::pose0.y(),0.0));
initialGuess.insert(x1,Pose2(simpleLago::pose1.x(),simpleLago::pose1.y(),0.0));
initialGuess.insert(x2,Pose2(simpleLago::pose2.x(),simpleLago::pose2.y(),0.0));
initialGuess.insert(x3,Pose2(simpleLago::pose3.x(),simpleLago::pose3.y(),0.0));
// lago does not touch the Cartesian part and only fixed the orientations
Values actual = lago::initialize(simple::graph(), initialGuess);
Values actual = lago::initialize(simpleLago::graph(), initialGuess);
// we are in a noiseless case
Values expected;
expected.insert(x0,simple::pose0);
expected.insert(x1,simple::pose1);
expected.insert(x2,simple::pose2);
expected.insert(x3,simple::pose3);
expected.insert(x0,simpleLago::pose0);
expected.insert(x1,simpleLago::pose1);
expected.insert(x2,simpleLago::pose2);
expected.insert(x3,simpleLago::pose3);
EXPECT(assert_equal(expected, actual, 1e-6));
}
@ -243,14 +243,14 @@ TEST( Lago, smallGraphValues ) {
TEST( Lago, smallGraph2 ) {
// lago does not touch the Cartesian part and only fixed the orientations
Values actual = lago::initialize(simple::graph());
Values actual = lago::initialize(simpleLago::graph());
// we are in a noiseless case
Values expected;
expected.insert(x0,simple::pose0);
expected.insert(x1,simple::pose1);
expected.insert(x2,simple::pose2);
expected.insert(x3,simple::pose3);
expected.insert(x0,simpleLago::pose0);
expected.insert(x1,simpleLago::pose1);
expected.insert(x2,simpleLago::pose2);
expected.insert(x3,simpleLago::pose3);
EXPECT(assert_equal(expected, actual, 1e-6));
}

21
gtsam/slam/tests/testPoseRotationPrior.cpp
View File

@ -35,6 +35,7 @@ const Rot3 rot3A, rot3B = Rot3::pitch(-M_PI_2), rot3C = Rot3::Expmap((Vector(3)
// Pose2 examples
const Point2 point2A(1.0, 2.0), point2B(4.0, 6.0);
const Rot2 rot2A, rot2B = Rot2::fromAngle(M_PI_2);
const Rot2 rot2C = Rot2::fromAngle(M_PI-0.01), rot2D = Rot2::fromAngle(M_PI+0.01);
/* ************************************************************************* */
Vector evalFactorError3(const Pose3RotationPrior& factor, const Pose3& x) {
@ -61,9 +62,15 @@ TEST( testPoseRotationFactor, level3_error ) {
Pose3 pose1(rot3A, point3A);
Pose3RotationPrior factor(poseKey, rot3C, model3);
Matrix actH1;
EXPECT(assert_equal((Vector(3) << -0.1,-0.2,-0.3), factor.evaluateError(pose1, actH1)));
#if defined(GTSAM_ROT3_EXPMAP) || defined(GTSAM_USE_QUATERNIONS)
EXPECT(assert_equal((Vector(3) << -0.1, -0.2,-0.3), factor.evaluateError(pose1, actH1)));
#else
EXPECT(assert_equal((Vector(3) << -0.1, -0.2, -0.3), factor.evaluateError(pose1, actH1),1e-2));
#endif
Matrix expH1 = numericalDerivative22(evalFactorError3, factor, pose1, 1e-5);
EXPECT(assert_equal(expH1, actH1, tol));
// the derivative is more complex, but is close to the identity for Rot3 around the origin
// If not using true expmap will be close, but not exact around the origin
// EXPECT(assert_equal(expH1, actH1, tol));
}
/* ************************************************************************* */
@ -86,6 +93,16 @@ TEST( testPoseRotationFactor, level2_error ) {
EXPECT(assert_equal(expH1, actH1, tol));
}
/* ************************************************************************* */
TEST( testPoseRotationFactor, level2_error_wrap ) {
Pose2 pose1(rot2C, point2A);
Pose2RotationPrior factor(poseKey, rot2D, model1);
Matrix actH1;
EXPECT(assert_equal((Vector(1) << -0.02), factor.evaluateError(pose1, actH1)));
Matrix expH1 = numericalDerivative22(evalFactorError2, factor, pose1, 1e-5);
EXPECT(assert_equal(expH1, actH1, tol));
}
/* ************************************************************************* */
int main() { TestResult tr; return TestRegistry::runAllTests(tr); }
/* ************************************************************************* */

138
gtsam_unstable/base/DSFMap.h
View File

@ -34,89 +34,89 @@ class DSFMap {
protected:
/// We store the forest in an STL map, but parents are done with pointers
struct Entry {
typename std::map<KEY, Entry>::iterator parent_;
size_t rank_;
Entry() {}
};
/// We store the forest in an STL map, but parents are done with pointers
struct Entry {
typename std::map<KEY, Entry>::iterator parent_;
size_t rank_;
Entry() {}
};
typedef typename std::map<KEY, Entry> Map;
typedef typename Map::iterator iterator;
mutable Map entries_;
typedef typename Map::iterator iterator;
mutable Map entries_;
/// Given key, find iterator to initial entry
iterator find__(const KEY& key) const {
static const Entry empty;
iterator it = entries_.find(key);
// if key does not exist, create and return itself
if (it == entries_.end()) {
it = entries_.insert(std::make_pair(key, empty)).first;
it->second.parent_ = it;
it->second.rank_ = 0;
}
return it;
}
/// Given key, find iterator to initial entry
iterator find__(const KEY& key) const {
static const Entry empty;
iterator it = entries_.find(key);
// if key does not exist, create and return itself
if (it == entries_.end()) {
it = entries_.insert(std::make_pair(key, empty)).first;
it->second.parent_ = it;
it->second.rank_ = 0;
}
return it;
}
/// Given iterator to initial entry, find the root Entry
iterator find_(const iterator& it) const {
// follow parent pointers until we reach set representative
iterator& parent = it->second.parent_;
if (parent != it)
parent = find_(parent); // not yet, recurse!
return parent;
}
/// Given iterator to initial entry, find the root Entry
iterator find_(const iterator& it) const {
// follow parent pointers until we reach set representative
iterator& parent = it->second.parent_;
if (parent != it)
parent = find_(parent); // not yet, recurse!
return parent;
}
/// Given key, find the root Entry
inline iterator find_(const KEY& key) const {
iterator initial = find__(key);
return find_(initial);
}
/// Given key, find the root Entry
inline iterator find_(const KEY& key) const {
iterator initial = find__(key);
return find_(initial);
}
public:
typedef std::set<KEY> Set;
typedef std::set<KEY> Set;
/// constructor
DSFMap() {
}
/// constructor
DSFMap() {
}
/// Given key, find the representative key for the set in which it lives
inline KEY find(const KEY& key) const {
iterator root = find_(key);
return root->first;
}
/// Given key, find the representative key for the set in which it lives
inline KEY find(const KEY& key) const {
iterator root = find_(key);
return root->first;
}
/// Merge two sets
void merge(const KEY& x, const KEY& y) {
/// Merge two sets
void merge(const KEY& x, const KEY& y) {
// straight from http://en.wikipedia.org/wiki/Disjoint-set_data_structure
iterator xRoot = find_(x);
iterator yRoot = find_(y);
if (xRoot == yRoot)
return;
// straight from http://en.wikipedia.org/wiki/Disjoint-set_data_structure
iterator xRoot = find_(x);
iterator yRoot = find_(y);
if (xRoot == yRoot)
return;
// Merge sets
if (xRoot->second.rank_ < yRoot->second.rank_)
xRoot->second.parent_ = yRoot;
else if (xRoot->second.rank_ > yRoot->second.rank_)
yRoot->second.parent_ = xRoot;
else {
yRoot->second.parent_ = xRoot;
xRoot->second.rank_ = xRoot->second.rank_ + 1;
}
}
// Merge sets
if (xRoot->second.rank_ < yRoot->second.rank_)
xRoot->second.parent_ = yRoot;
else if (xRoot->second.rank_ > yRoot->second.rank_)
yRoot->second.parent_ = xRoot;
else {
yRoot->second.parent_ = xRoot;
xRoot->second.rank_ = xRoot->second.rank_ + 1;
}
}
/// return all sets, i.e. a partition of all elements
std::map<KEY, Set> sets() const {
std::map<KEY, Set> sets;
iterator it = entries_.begin();
for (; it != entries_.end(); it++) {
iterator root = find_(it);
sets[root->first].insert(it->first);
}
return sets;
}
/// return all sets, i.e. a partition of all elements
std::map<KEY, Set> sets() const {
std::map<KEY, Set> sets;
iterator it = entries_.begin();
for (; it != entries_.end(); it++) {
iterator root = find_(it);
sets[root->first].insert(it->first);
}
return sets;
}
};

4
gtsam_unstable/discrete/tests/testLoopyBelief.cpp
View File

@ -1,8 +1,8 @@
/**
* @file testLoopyBelief.cpp
* @file testLoopyBelief.cpp
* @brief
* @author Duy-Nguyen Ta
* @date Oct 11, 2013
* @date Oct 11, 2013
*/
#include <gtsam/inference/VariableIndex.h>

910
gtsam_unstable/partition/FindSeparator-inl.h
View File

@ -26,539 +26,539 @@ extern "C" {
namespace gtsam { namespace partition {
typedef boost::shared_array<idx_t> sharedInts;
typedef boost::shared_array<idx_t> sharedInts;
/* ************************************************************************* */
/**
* Return the size of the separator and the partiion indices {part}
* Part [j] is 0, 1, or 2, depending on
* whether node j is in the left part of the graph, the right part, or the
* separator, respectively
*/
std::pair<int, sharedInts> separatorMetis(idx_t n, const sharedInts& xadj,
const sharedInts& adjncy, const sharedInts& adjwgt, bool verbose) {
/* ************************************************************************* */
/**
* Return the size of the separator and the partiion indices {part}
* Part [j] is 0, 1, or 2, depending on
* whether node j is in the left part of the graph, the right part, or the
* separator, respectively
*/
std::pair<int, sharedInts> separatorMetis(idx_t n, const sharedInts& xadj,
const sharedInts& adjncy, const sharedInts& adjwgt, bool verbose) {
// control parameters
idx_t vwgt[n]; // the weights of the vertices
idx_t options[METIS_NOPTIONS];
METIS_SetDefaultOptions(options); // use defaults
idx_t sepsize; // the size of the separator, output
sharedInts part_(new idx_t[n]); // the partition of each vertex, output
// control parameters
idx_t vwgt[n]; // the weights of the vertices
idx_t options[METIS_NOPTIONS];
METIS_SetDefaultOptions(options); // use defaults
idx_t sepsize; // the size of the separator, output
sharedInts part_(new idx_t[n]); // the partition of each vertex, output
// set uniform weights on the vertices
std::fill(vwgt, vwgt+n, 1);
// set uniform weights on the vertices
std::fill(vwgt, vwgt+n, 1);
// TODO: Fix at later time
//boost::timer::cpu_timer TOTALTmr;
if (verbose) {
printf("**********************************************************************\n");
printf("Graph Information ---------------------------------------------------\n");
printf(" #Vertices: %d, #Edges: %u\n", n, *(xadj.get()+n) / 2);
printf("\nND Partitioning... -------------------------------------------\n");
//TOTALTmr.start()
}
// TODO: Fix at later time
//boost::timer::cpu_timer TOTALTmr;
if (verbose) {
printf("**********************************************************************\n");
printf("Graph Information ---------------------------------------------------\n");
printf(" #Vertices: %d, #Edges: %u\n", n, *(xadj.get()+n) / 2);
printf("\nND Partitioning... -------------------------------------------\n");
//TOTALTmr.start()
}
// call metis parition routine
METIS_ComputeVertexSeparator(&n, xadj.get(), adjncy.get(),
// call metis parition routine
METIS_ComputeVertexSeparator(&n, xadj.get(), adjncy.get(),
vwgt, options, &sepsize, part_.get());
if (verbose) {
//boost::cpu_times const elapsed_times(timer.elapsed());
//printf("\nTiming Information --------------------------------------------------\n");
//printf(" Total: \t\t %7.3f\n", elapsed_times);
printf(" Sep size: \t\t %d\n", sepsize);
printf("**********************************************************************\n");
}
if (verbose) {
//boost::cpu_times const elapsed_times(timer.elapsed());
//printf("\nTiming Information --------------------------------------------------\n");
//printf(" Total: \t\t %7.3f\n", elapsed_times);
printf(" Sep size: \t\t %d\n", sepsize);
printf("**********************************************************************\n");
}
return std::make_pair(sepsize, part_);
}
return std::make_pair(sepsize, part_);
}
/* ************************************************************************* */
void modefied_EdgeComputeSeparator(idx_t *nvtxs, idx_t *xadj, idx_t *adjncy, idx_t *vwgt,
idx_t *adjwgt, idx_t *options, idx_t *edgecut, idx_t *part)
{
/* ************************************************************************* */
void modefied_EdgeComputeSeparator(idx_t *nvtxs, idx_t *xadj, idx_t *adjncy, idx_t *vwgt,
idx_t *adjwgt, idx_t *options, idx_t *edgecut, idx_t *part)
{
idx_t i, ncon;
graph_t *graph;
real_t *tpwgts2;
ctrl_t *ctrl;
ctrl = SetupCtrl(METIS_OP_OMETIS, options, 1, 3, NULL, NULL);
ctrl->iptype = METIS_IPTYPE_GROW;
//if () == NULL)
// return METIS_ERROR_INPUT;
graph_t *graph;
real_t *tpwgts2;
ctrl_t *ctrl;
ctrl = SetupCtrl(METIS_OP_OMETIS, options, 1, 3, NULL, NULL);
ctrl->iptype = METIS_IPTYPE_GROW;
//if () == NULL)
// return METIS_ERROR_INPUT;
InitRandom(ctrl->seed);
InitRandom(ctrl->seed);
graph = SetupGraph(ctrl, *nvtxs, 1, xadj, adjncy, vwgt, NULL, NULL);
graph = SetupGraph(ctrl, *nvtxs, 1, xadj, adjncy, vwgt, NULL, NULL);
AllocateWorkSpace(ctrl, graph);
AllocateWorkSpace(ctrl, graph);
ncon = graph->ncon;
ctrl->ncuts = 1;
/* determine the weights of the two partitions as a function of the weight of the
target partition weights */
ncon = graph->ncon;
ctrl->ncuts = 1;
/* determine the weights of the two partitions as a function of the weight of the
target partition weights */
tpwgts2 = rwspacemalloc(ctrl, 2*ncon);
for (i=0; i<ncon; i++) {
tpwgts2[i] = rsum((2>>1), ctrl->tpwgts+i, ncon);
tpwgts2[ncon+i] = 1.0 - tpwgts2[i];
}
/* perform the bisection */
*edgecut = MultilevelBisect(ctrl, graph, tpwgts2);
tpwgts2 = rwspacemalloc(ctrl, 2*ncon);
for (i=0; i<ncon; i++) {
tpwgts2[i] = rsum((2>>1), ctrl->tpwgts+i, ncon);
tpwgts2[ncon+i] = 1.0 - tpwgts2[i];
}
/* perform the bisection */
*edgecut = MultilevelBisect(ctrl, graph, tpwgts2);
// ConstructMinCoverSeparator(&ctrl, &graph, 1.05);
// *edgecut = graph->mincut;
// *sepsize = graph.pwgts[2];
icopy(*nvtxs, graph->where, part);
std::cout << "Finished bisection:" << *edgecut << std::endl;
FreeGraph(&graph);
// ConstructMinCoverSeparator(&ctrl, &graph, 1.05);
// *edgecut = graph->mincut;
// *sepsize = graph.pwgts[2];
icopy(*nvtxs, graph->where, part);
std::cout << "Finished bisection:" << *edgecut << std::endl;
FreeGraph(&graph);
FreeCtrl(&ctrl);
}
FreeCtrl(&ctrl);
}
/* ************************************************************************* */
/**
* Return the number of edge cuts and the partition indices {part}
* Part [j] is 0 or 1, depending on
* whether node j is in the left part of the graph or the right part respectively
*/
std::pair<int, sharedInts> edgeMetis(idx_t n, const sharedInts& xadj, const sharedInts& adjncy,
const sharedInts& adjwgt, bool verbose) {
/* ************************************************************************* */
/**
* Return the number of edge cuts and the partition indices {part}
* Part [j] is 0 or 1, depending on
* whether node j is in the left part of the graph or the right part respectively
*/
std::pair<int, sharedInts> edgeMetis(idx_t n, const sharedInts& xadj, const sharedInts& adjncy,
const sharedInts& adjwgt, bool verbose) {
// control parameters
idx_t vwgt[n]; // the weights of the vertices
idx_t options[METIS_NOPTIONS];
METIS_SetDefaultOptions(options); // use defaults
idx_t edgecut; // the number of edge cuts, output
sharedInts part_(new idx_t[n]); // the partition of each vertex, output
// control parameters
idx_t vwgt[n]; // the weights of the vertices
idx_t options[METIS_NOPTIONS];
METIS_SetDefaultOptions(options); // use defaults
idx_t edgecut; // the number of edge cuts, output
sharedInts part_(new idx_t[n]); // the partition of each vertex, output
// set uniform weights on the vertices
std::fill(vwgt, vwgt+n, 1);
// set uniform weights on the vertices
std::fill(vwgt, vwgt+n, 1);
//TODO: Fix later
//boost::timer TOTALTmr;
if (verbose) {
printf("**********************************************************************\n");
printf("Graph Information ---------------------------------------------------\n");
printf(" #Vertices: %d, #Edges: %u\n", n, *(xadj.get()+n) / 2);
printf("\nND Partitioning... -------------------------------------------\n");
//cleartimer(TOTALTmr);
//starttimer(TOTALTmr);
}
//TODO: Fix later
//boost::timer TOTALTmr;
if (verbose) {
printf("**********************************************************************\n");
printf("Graph Information ---------------------------------------------------\n");
printf(" #Vertices: %d, #Edges: %u\n", n, *(xadj.get()+n) / 2);
printf("\nND Partitioning... -------------------------------------------\n");
//cleartimer(TOTALTmr);
//starttimer(TOTALTmr);
}
//int wgtflag = 1; // only edge weights
//int numflag = 0; // c style numbering starting from 0
//int nparts = 2; // partition the graph to 2 submaps
modefied_EdgeComputeSeparator(&n, xadj.get(), adjncy.get(), vwgt, adjwgt.get(),
options, &edgecut, part_.get());
//int wgtflag = 1; // only edge weights
//int numflag = 0; // c style numbering starting from 0
//int nparts = 2; // partition the graph to 2 submaps
modefied_EdgeComputeSeparator(&n, xadj.get(), adjncy.get(), vwgt, adjwgt.get(),
options, &edgecut, part_.get());
if (verbose) {
//stoptimer(TOTALTmr);
printf("\nTiming Information --------------------------------------------------\n");
//printf(" Total: \t\t %7.3f\n", gettimer(TOTALTmr));
printf(" Edge cuts: \t\t %d\n", edgecut);
printf("**********************************************************************\n");
}
if (verbose) {
//stoptimer(TOTALTmr);
printf("\nTiming Information --------------------------------------------------\n");
//printf(" Total: \t\t %7.3f\n", gettimer(TOTALTmr));
printf(" Edge cuts: \t\t %d\n", edgecut);
printf("**********************************************************************\n");
}
return std::make_pair(edgecut, part_);
}
return std::make_pair(edgecut, part_);
}
/* ************************************************************************* */
/**
* Prepare the data structure {xadj} and {adjncy} required by metis
* xadj always has the size equal to the no. of the nodes plus 1
* adjncy always has the size equal to two times of the no. of the edges in the Metis graph
*/
template <class GenericGraph>
void prepareMetisGraph(const GenericGraph& graph, const std::vector<size_t>& keys, WorkSpace& workspace,
sharedInts* ptr_xadj, sharedInts* ptr_adjncy, sharedInts* ptr_adjwgt) {
/* ************************************************************************* */
/**
* Prepare the data structure {xadj} and {adjncy} required by metis
* xadj always has the size equal to the no. of the nodes plus 1
* adjncy always has the size equal to two times of the no. of the edges in the Metis graph
*/
template <class GenericGraph>
void prepareMetisGraph(const GenericGraph& graph, const std::vector<size_t>& keys, WorkSpace& workspace,
sharedInts* ptr_xadj, sharedInts* ptr_adjncy, sharedInts* ptr_adjwgt) {
typedef int Weight;
typedef std::vector<int> Weights;
typedef std::vector<int> Neighbors;
typedef std::pair<Neighbors, Weights> NeighborsInfo;
typedef int Weight;
typedef std::vector<int> Weights;
typedef std::vector<int> Neighbors;
typedef std::pair<Neighbors, Weights> NeighborsInfo;
// set up dictionary
std::vector<int>& dictionary = workspace.dictionary;
workspace.prepareDictionary(keys);
// set up dictionary
std::vector<int>& dictionary = workspace.dictionary;
workspace.prepareDictionary(keys);
// prepare for {adjacencyMap}, a pair of neighbor indices and the correponding edge weights
int numNodes = keys.size();
int numEdges = 0;
std::vector<NeighborsInfo> adjacencyMap;
adjacencyMap.resize(numNodes);
std::cout << "Number of nodes: " << adjacencyMap.size() << std::endl;
int index1, index2;
// prepare for {adjacencyMap}, a pair of neighbor indices and the correponding edge weights
int numNodes = keys.size();
int numEdges = 0;
std::vector<NeighborsInfo> adjacencyMap;
adjacencyMap.resize(numNodes);
std::cout << "Number of nodes: " << adjacencyMap.size() << std::endl;
int index1, index2;
BOOST_FOREACH(const typename GenericGraph::value_type& factor, graph){
index1 = dictionary[factor->key1.index];
index2 = dictionary[factor->key2.index];
std::cout << "index1: " << index1 << std::endl;
std::cout << "index2: " << index2 << std::endl;
// if both nodes are in the current graph, i.e. not a joint factor between frontal and separator
if (index1 >= 0 && index2 >= 0) {
std::pair<Neighbors, Weights>& adjacencyMap1 = adjacencyMap[index1];
std::pair<Neighbors, Weights>& adjacencyMap2 = adjacencyMap[index2];
try{
adjacencyMap1.first.push_back(index2);
adjacencyMap1.second.push_back(factor->weight);
adjacencyMap2.first.push_back(index1);
adjacencyMap2.second.push_back(factor->weight);
}catch(std::exception& e){
std::cout << e.what() << std::endl;
}
numEdges++;
}
}
BOOST_FOREACH(const typename GenericGraph::value_type& factor, graph){
index1 = dictionary[factor->key1.index];
index2 = dictionary[factor->key2.index];
std::cout << "index1: " << index1 << std::endl;
std::cout << "index2: " << index2 << std::endl;
// if both nodes are in the current graph, i.e. not a joint factor between frontal and separator
if (index1 >= 0 && index2 >= 0) {
std::pair<Neighbors, Weights>& adjacencyMap1 = adjacencyMap[index1];
std::pair<Neighbors, Weights>& adjacencyMap2 = adjacencyMap[index2];
try{
adjacencyMap1.first.push_back(index2);
adjacencyMap1.second.push_back(factor->weight);
adjacencyMap2.first.push_back(index1);
adjacencyMap2.second.push_back(factor->weight);
}catch(std::exception& e){
std::cout << e.what() << std::endl;
}
numEdges++;
}
}
// prepare for {xadj}, {adjncy}, and {adjwgt}
*ptr_xadj = sharedInts(new idx_t[numNodes+1]);
*ptr_adjncy = sharedInts(new idx_t[numEdges*2]);
*ptr_adjwgt = sharedInts(new idx_t[numEdges*2]);
sharedInts& xadj = *ptr_xadj;
sharedInts& adjncy = *ptr_adjncy;
sharedInts& adjwgt = *ptr_adjwgt;
int ind_xadj = 0, ind_adjncy = 0;
BOOST_FOREACH(const NeighborsInfo& info, adjacencyMap) {
*(xadj.get() + ind_xadj) = ind_adjncy;
std::copy(info.first .begin(), info.first .end(), adjncy.get() + ind_adjncy);
std::copy(info.second.begin(), info.second.end(), adjwgt.get() + ind_adjncy);
assert(info.first.size() == info.second.size());
ind_adjncy += info.first.size();
ind_xadj ++;
}
if (ind_xadj != numNodes) throw std::runtime_error("prepareMetisGraph_: ind_xadj != numNodes");
*(xadj.get() + ind_xadj) = ind_adjncy;
}
// prepare for {xadj}, {adjncy}, and {adjwgt}
*ptr_xadj = sharedInts(new idx_t[numNodes+1]);
*ptr_adjncy = sharedInts(new idx_t[numEdges*2]);
*ptr_adjwgt = sharedInts(new idx_t[numEdges*2]);
sharedInts& xadj = *ptr_xadj;
sharedInts& adjncy = *ptr_adjncy;
sharedInts& adjwgt = *ptr_adjwgt;
int ind_xadj = 0, ind_adjncy = 0;
BOOST_FOREACH(const NeighborsInfo& info, adjacencyMap) {
*(xadj.get() + ind_xadj) = ind_adjncy;
std::copy(info.first .begin(), info.first .end(), adjncy.get() + ind_adjncy);
std::copy(info.second.begin(), info.second.end(), adjwgt.get() + ind_adjncy);
assert(info.first.size() == info.second.size());
ind_adjncy += info.first.size();
ind_xadj ++;
}
if (ind_xadj != numNodes) throw std::runtime_error("prepareMetisGraph_: ind_xadj != numNodes");
*(xadj.get() + ind_xadj) = ind_adjncy;
}
/* ************************************************************************* */
template<class GenericGraph>
boost::optional<MetisResult> separatorPartitionByMetis(const GenericGraph& graph,
const std::vector<size_t>& keys, WorkSpace& workspace, bool verbose) {
// create a metis graph
size_t numKeys = keys.size();
if (verbose)
std::cout << graph.size() << " factors,\t" << numKeys << " nodes;\t" << std::endl;
/* ************************************************************************* */
template<class GenericGraph>
boost::optional<MetisResult> separatorPartitionByMetis(const GenericGraph& graph,
const std::vector<size_t>& keys, WorkSpace& workspace, bool verbose) {
// create a metis graph
size_t numKeys = keys.size();
if (verbose)
std::cout << graph.size() << " factors,\t" << numKeys << " nodes;\t" << std::endl;
sharedInts xadj, adjncy, adjwgt;
sharedInts xadj, adjncy, adjwgt;
prepareMetisGraph<GenericGraph>(graph, keys, workspace, &xadj, &adjncy, &adjwgt);
prepareMetisGraph<GenericGraph>(graph, keys, workspace, &xadj, &adjncy, &adjwgt);
// run ND on the graph
size_t sepsize;
sharedInts part;
boost::tie(sepsize, part) = separatorMetis(numKeys, xadj, adjncy, adjwgt, verbose);
if (!sepsize) return boost::optional<MetisResult>();
// run ND on the graph
size_t sepsize;
sharedInts part;
boost::tie(sepsize, part) = separatorMetis(numKeys, xadj, adjncy, adjwgt, verbose);
if (!sepsize) return boost::optional<MetisResult>();
// convert the 0-1-2 from Metis to 1-2-0, so that the separator is 0, as later
// we will have more submaps
MetisResult result;
result.C.reserve(sepsize);
result.A.reserve(numKeys - sepsize);
result.B.reserve(numKeys - sepsize);
int* ptr_part = part.get();
std::vector<size_t>::const_iterator itKey = keys.begin();
std::vector<size_t>::const_iterator itKeyLast = keys.end();
while(itKey != itKeyLast) {
switch(*(ptr_part++)) {
case 0: result.A.push_back(*(itKey++)); break;
case 1: result.B.push_back(*(itKey++)); break;
case 2: result.C.push_back(*(itKey++)); break;
default: throw std::runtime_error("separatorPartitionByMetis: invalid results from Metis ND!");
}
}
// convert the 0-1-2 from Metis to 1-2-0, so that the separator is 0, as later
// we will have more submaps
MetisResult result;
result.C.reserve(sepsize);
result.A.reserve(numKeys - sepsize);
result.B.reserve(numKeys - sepsize);
int* ptr_part = part.get();
std::vector<size_t>::const_iterator itKey = keys.begin();
std::vector<size_t>::const_iterator itKeyLast = keys.end();
while(itKey != itKeyLast) {
switch(*(ptr_part++)) {
case 0: result.A.push_back(*(itKey++)); break;
case 1: result.B.push_back(*(itKey++)); break;
case 2: result.C.push_back(*(itKey++)); break;
default: throw std::runtime_error("separatorPartitionByMetis: invalid results from Metis ND!");
}
}
if (verbose) {
std::cout << "total key: " << keys.size()
<< " result(A,B,C) = " << result.A.size() << ", " << result.B.size() << ", "
<< result.C.size() << "; sepsize from Metis = " << sepsize << std::endl;
//throw runtime_error("separatorPartitionByMetis:stop for debug");
}
if (verbose) {
std::cout << "total key: " << keys.size()
<< " result(A,B,C) = " << result.A.size() << ", " << result.B.size() << ", "
<< result.C.size() << "; sepsize from Metis = " << sepsize << std::endl;
//throw runtime_error("separatorPartitionByMetis:stop for debug");
}
if(result.C.size() != sepsize) {
std::cout << "total key: " << keys.size()
<< " result(A,B,C) = " << result.A.size() << ", " << result.B.size() << ", " << result.C.size()
<< "; sepsize from Metis = " << sepsize << std::endl;
throw std::runtime_error("separatorPartitionByMetis: invalid sepsize from Metis ND!");
}
if(result.C.size() != sepsize) {
std::cout << "total key: " << keys.size()
<< " result(A,B,C) = " << result.A.size() << ", " << result.B.size() << ", " << result.C.size()
<< "; sepsize from Metis = " << sepsize << std::endl;
throw std::runtime_error("separatorPartitionByMetis: invalid sepsize from Metis ND!");
}
return boost::make_optional<MetisResult >(result);
}
return boost::make_optional<MetisResult >(result);
}
/* *************************************************************************/
template<class GenericGraph>
boost::optional<MetisResult> edgePartitionByMetis(const GenericGraph& graph,
const std::vector<size_t>& keys, WorkSpace& workspace, bool verbose) {
/* *************************************************************************/
template<class GenericGraph>
boost::optional<MetisResult> edgePartitionByMetis(const GenericGraph& graph,
const std::vector<size_t>& keys, WorkSpace& workspace, bool verbose) {
// a small hack for handling the camera1-camera2 case used in the unit tests
if (graph.size() == 1 && keys.size() == 2) {
MetisResult result;
result.A.push_back(keys.front());
result.B.push_back(keys.back());
return result;
}
// a small hack for handling the camera1-camera2 case used in the unit tests
if (graph.size() == 1 && keys.size() == 2) {
MetisResult result;
result.A.push_back(keys.front());
result.B.push_back(keys.back());
return result;
}
// create a metis graph
size_t numKeys = keys.size();
if (verbose) std::cout << graph.size() << " factors,\t" << numKeys << " nodes;\t" << std::endl;
sharedInts xadj, adjncy, adjwgt;
prepareMetisGraph<GenericGraph>(graph, keys, workspace, &xadj, &adjncy, &adjwgt);
// create a metis graph
size_t numKeys = keys.size();
if (verbose) std::cout << graph.size() << " factors,\t" << numKeys << " nodes;\t" << std::endl;
sharedInts xadj, adjncy, adjwgt;
prepareMetisGraph<GenericGraph>(graph, keys, workspace, &xadj, &adjncy, &adjwgt);
// run metis on the graph
int edgecut;
sharedInts part;
boost::tie(edgecut, part) = edgeMetis(numKeys, xadj, adjncy, adjwgt, verbose);
// run metis on the graph
int edgecut;
sharedInts part;
boost::tie(edgecut, part) = edgeMetis(numKeys, xadj, adjncy, adjwgt, verbose);
// convert the 0-1-2 from Metis to 1-2-0, so that the separator is 0, as later we will have more submaps
MetisResult result;
result.A.reserve(numKeys);
result.B.reserve(numKeys);
int* ptr_part = part.get();
std::vector<size_t>::const_iterator itKey = keys.begin();
std::vector<size_t>::const_iterator itKeyLast = keys.end();
while(itKey != itKeyLast) {
if (*ptr_part != 0 && *ptr_part != 1)
std::cout << *ptr_part << "!!!" << std::endl;
switch(*(ptr_part++)) {
case 0: result.A.push_back(*(itKey++)); break;
case 1: result.B.push_back(*(itKey++)); break;
default: throw std::runtime_error("edgePartitionByMetis: invalid results from Metis ND!");
}
}
// convert the 0-1-2 from Metis to 1-2-0, so that the separator is 0, as later we will have more submaps
MetisResult result;
result.A.reserve(numKeys);
result.B.reserve(numKeys);
int* ptr_part = part.get();
std::vector<size_t>::const_iterator itKey = keys.begin();
std::vector<size_t>::const_iterator itKeyLast = keys.end();
while(itKey != itKeyLast) {
if (*ptr_part != 0 && *ptr_part != 1)
std::cout << *ptr_part << "!!!" << std::endl;
switch(*(ptr_part++)) {
case 0: result.A.push_back(*(itKey++)); break;
case 1: result.B.push_back(*(itKey++)); break;
default: throw std::runtime_error("edgePartitionByMetis: invalid results from Metis ND!");
}
}
if (verbose) {
std::cout << "the size of two submaps in the reduced graph: " << result.A.size()
<< " " << result.B.size() << std::endl;
int edgeCut = 0;
if (verbose) {
std::cout << "the size of two submaps in the reduced graph: " << result.A.size()
<< " " << result.B.size() << std::endl;
int edgeCut = 0;
BOOST_FOREACH(const typename GenericGraph::value_type& factor, graph){
int key1 = factor->key1.index;
int key2 = factor->key2.index;
// print keys and their subgraph assignment
std::cout << key1;
if (std::find(result.A.begin(), result.A.end(), key1) != result.A.end()) std::cout <<"A ";
if (std::find(result.B.begin(), result.B.end(), key1) != result.B.end()) std::cout <<"B ";
BOOST_FOREACH(const typename GenericGraph::value_type& factor, graph){
int key1 = factor->key1.index;
int key2 = factor->key2.index;
// print keys and their subgraph assignment
std::cout << key1;
if (std::find(result.A.begin(), result.A.end(), key1) != result.A.end()) std::cout <<"A ";
if (std::find(result.B.begin(), result.B.end(), key1) != result.B.end()) std::cout <<"B ";
std::cout << key2;
if (std::find(result.A.begin(), result.A.end(), key2) != result.A.end()) std::cout <<"A ";
if (std::find(result.B.begin(), result.B.end(), key2) != result.B.end()) std::cout <<"B ";
std::cout << key2;
if (std::find(result.A.begin(), result.A.end(), key2) != result.A.end()) std::cout <<"A ";
if (std::find(result.B.begin(), result.B.end(), key2) != result.B.end()) std::cout <<"B ";
std::cout << "weight " << factor->weight;;
// find vertices that were assigned to sets A & B. Their edge will be cut
if ((std::find(result.A.begin(), result.A.end(), key1) != result.A.end() &&
std::find(result.B.begin(), result.B.end(), key2) != result.B.end()) ||
(std::find(result.B.begin(), result.B.end(), key1) != result.B.end() &&
std::find(result.A.begin(), result.A.end(), key2) != result.A.end())){
edgeCut ++;
std::cout << " CUT ";
}
std::cout << std::endl;
}
std::cout << "edgeCut: " << edgeCut << std::endl;
}
// find vertices that were assigned to sets A & B. Their edge will be cut
if ((std::find(result.A.begin(), result.A.end(), key1) != result.A.end() &&
std::find(result.B.begin(), result.B.end(), key2) != result.B.end()) ||
(std::find(result.B.begin(), result.B.end(), key1) != result.B.end() &&
std::find(result.A.begin(), result.A.end(), key2) != result.A.end())){
edgeCut ++;
std::cout << " CUT ";
}
std::cout << std::endl;
}
std::cout << "edgeCut: " << edgeCut << std::endl;
}
return boost::make_optional<MetisResult >(result);
}
return boost::make_optional<MetisResult >(result);
}
/* ************************************************************************* */
bool isLargerIsland(const std::vector<size_t>& island1, const std::vector<size_t>& island2) {
return island1.size() > island2.size();
}
/* ************************************************************************* */
bool isLargerIsland(const std::vector<size_t>& island1, const std::vector<size_t>& island2) {
return island1.size() > island2.size();
}
/* ************************************************************************* */
// debug functions
void printIsland(const std::vector<size_t>& island) {
std::cout << "island: ";
BOOST_FOREACH(const size_t key, island)
std::cout << key << " ";
std::cout << std::endl;
}
/* ************************************************************************* */
// debug functions
void printIsland(const std::vector<size_t>& island) {
std::cout << "island: ";
BOOST_FOREACH(const size_t key, island)
std::cout << key << " ";
std::cout << std::endl;
}
void printIslands(const std::list<std::vector<size_t> >& islands) {
BOOST_FOREACH(const std::vector<std::size_t>& island, islands)
printIsland(island);
}
void printIslands(const std::list<std::vector<size_t> >& islands) {
BOOST_FOREACH(const std::vector<std::size_t>& island, islands)
printIsland(island);
}
void printNumCamerasLandmarks(const std::vector<size_t>& keys, const std::vector<Symbol>& int2symbol) {
int numCamera = 0, numLandmark = 0;
BOOST_FOREACH(const size_t key, keys)
if (int2symbol[key].chr() == 'x')
numCamera++;
else
numLandmark++;
std::cout << "numCamera: " << numCamera << " numLandmark: " << numLandmark << std::endl;
}
void printNumCamerasLandmarks(const std::vector<size_t>& keys, const std::vector<Symbol>& int2symbol) {
int numCamera = 0, numLandmark = 0;
BOOST_FOREACH(const size_t key, keys)
if (int2symbol[key].chr() == 'x')
numCamera++;
else
numLandmark++;
std::cout << "numCamera: " << numCamera << " numLandmark: " << numLandmark << std::endl;
}
/* ************************************************************************* */
template<class GenericGraph>
void addLandmarkToPartitionResult(const GenericGraph& graph, const std::vector<size_t>& landmarkKeys,
MetisResult& partitionResult, WorkSpace& workspace) {
/* ************************************************************************* */
template<class GenericGraph>
void addLandmarkToPartitionResult(const GenericGraph& graph, const std::vector<size_t>& landmarkKeys,
MetisResult& partitionResult, WorkSpace& workspace) {
// set up cameras in the dictionary
std::vector<size_t>& A = partitionResult.A;
std::vector<size_t>& B = partitionResult.B;
std::vector<size_t>& C = partitionResult.C;
std::vector<int>& dictionary = workspace.dictionary;
std::fill(dictionary.begin(), dictionary.end(), -1);
BOOST_FOREACH(const size_t a, A)
dictionary[a] = 1;
BOOST_FOREACH(const size_t b, B)
dictionary[b] = 2;
if (!C.empty())
throw std::runtime_error("addLandmarkToPartitionResult: C is not empty");
// set up cameras in the dictionary
std::vector<size_t>& A = partitionResult.A;
std::vector<size_t>& B = partitionResult.B;
std::vector<size_t>& C = partitionResult.C;
std::vector<int>& dictionary = workspace.dictionary;
std::fill(dictionary.begin(), dictionary.end(), -1);
BOOST_FOREACH(const size_t a, A)
dictionary[a] = 1;
BOOST_FOREACH(const size_t b, B)
dictionary[b] = 2;
if (!C.empty())
throw std::runtime_error("addLandmarkToPartitionResult: C is not empty");
// set up landmarks
size_t i,j;
BOOST_FOREACH(const typename GenericGraph::value_type& factor, graph) {
i = factor->key1.index;
j = factor->key2.index;
if (dictionary[j] == 0) // if the landmark is already in the separator, continue
continue;
else if (dictionary[j] == -1)
dictionary[j] = dictionary[i];
else {
if (dictionary[j] != dictionary[i])
dictionary[j] = 0;
}
// if (j == 67980)
// std::cout << "dictionary[67980]" << dictionary[j] << std::endl;
}
// set up landmarks
size_t i,j;
BOOST_FOREACH(const typename GenericGraph::value_type& factor, graph) {
i = factor->key1.index;
j = factor->key2.index;
if (dictionary[j] == 0) // if the landmark is already in the separator, continue
continue;
else if (dictionary[j] == -1)
dictionary[j] = dictionary[i];
else {
if (dictionary[j] != dictionary[i])
dictionary[j] = 0;
}
// if (j == 67980)
// std::cout << "dictionary[67980]" << dictionary[j] << std::endl;
}
BOOST_FOREACH(const size_t j, landmarkKeys) {
switch(dictionary[j]) {
case 0: C.push_back(j); break;
case 1: A.push_back(j); break;
case 2: B.push_back(j); break;
default: std::cout << j << ": " << dictionary[j] << std::endl;
throw std::runtime_error("addLandmarkToPartitionResult: wrong status for landmark");
}
}
}
BOOST_FOREACH(const size_t j, landmarkKeys) {
switch(dictionary[j]) {
case 0: C.push_back(j); break;
case 1: A.push_back(j); break;
case 2: B.push_back(j); break;
default: std::cout << j << ": " << dictionary[j] << std::endl;
throw std::runtime_error("addLandmarkToPartitionResult: wrong status for landmark");
}
}
}
#define REDUCE_CAMERA_GRAPH
/* ************************************************************************* */
template<class GenericGraph>
boost::optional<MetisResult> findPartitoning(const GenericGraph& graph, const std::vector<size_t>& keys,
WorkSpace& workspace, bool verbose,
const boost::optional<std::vector<Symbol> >& int2symbol, const bool reduceGraph) {
boost::optional<MetisResult> result;
GenericGraph reducedGraph;
std::vector<size_t> keyToPartition;
std::vector<size_t> cameraKeys, landmarkKeys;
if (reduceGraph) {
if (!int2symbol.is_initialized())
throw std::invalid_argument("findSeparator: int2symbol must be valid!");
/* ************************************************************************* */
template<class GenericGraph>
boost::optional<MetisResult> findPartitoning(const GenericGraph& graph, const std::vector<size_t>& keys,
WorkSpace& workspace, bool verbose,
const boost::optional<std::vector<Symbol> >& int2symbol, const bool reduceGraph) {
boost::optional<MetisResult> result;
GenericGraph reducedGraph;
std::vector<size_t> keyToPartition;
std::vector<size_t> cameraKeys, landmarkKeys;
if (reduceGraph) {
if (!int2symbol.is_initialized())
throw std::invalid_argument("findSeparator: int2symbol must be valid!");
// find out all the landmark keys, which are to be eliminated
cameraKeys.reserve(keys.size());
landmarkKeys.reserve(keys.size());
BOOST_FOREACH(const size_t key, keys) {
if((*int2symbol)[key].chr() == 'x')
cameraKeys.push_back(key);
else
landmarkKeys.push_back(key);
}
// find out all the landmark keys, which are to be eliminated
cameraKeys.reserve(keys.size());
landmarkKeys.reserve(keys.size());
BOOST_FOREACH(const size_t key, keys) {
if((*int2symbol)[key].chr() == 'x')
cameraKeys.push_back(key);
else
landmarkKeys.push_back(key);
}
keyToPartition = cameraKeys;
workspace.prepareDictionary(keyToPartition);
const std::vector<int>& dictionary = workspace.dictionary;
reduceGenericGraph(graph, cameraKeys, landmarkKeys, dictionary, reducedGraph);
std::cout << "original graph: V" << keys.size() << ", E" << graph.size()
<< " --> reduced graph: V" << cameraKeys.size() << ", E" << reducedGraph.size() << std::endl;
result = edgePartitionByMetis(reducedGraph, keyToPartition, workspace, verbose);
} else // call Metis to partition the graph to A, B, C
result = separatorPartitionByMetis(graph, keys, workspace, verbose);
keyToPartition = cameraKeys;
workspace.prepareDictionary(keyToPartition);
const std::vector<int>& dictionary = workspace.dictionary;
reduceGenericGraph(graph, cameraKeys, landmarkKeys, dictionary, reducedGraph);
std::cout << "original graph: V" << keys.size() << ", E" << graph.size()
<< " --> reduced graph: V" << cameraKeys.size() << ", E" << reducedGraph.size() << std::endl;
result = edgePartitionByMetis(reducedGraph, keyToPartition, workspace, verbose);
} else // call Metis to partition the graph to A, B, C
result = separatorPartitionByMetis(graph, keys, workspace, verbose);
if (!result.is_initialized()) {
std::cout << "metis failed!" << std::endl;
return 0;
}
if (!result.is_initialized()) {
std::cout << "metis failed!" << std::endl;
return 0;
}
if (reduceGraph) {
addLandmarkToPartitionResult(graph, landmarkKeys, *result, workspace);
std::cout << "the separator size: " << result->C.size() << " landmarks" << std::endl;
}
if (reduceGraph) {
addLandmarkToPartitionResult(graph, landmarkKeys, *result, workspace);
std::cout << "the separator size: " << result->C.size() << " landmarks" << std::endl;
}
return result;
}
return result;
}
/* ************************************************************************* */
template<class GenericGraph>
int findSeparator(const GenericGraph& graph, const std::vector<size_t>& keys,
const int minNodesPerMap, WorkSpace& workspace, bool verbose,
const boost::optional<std::vector<Symbol> >& int2symbol, const bool reduceGraph,
const int minNrConstraintsPerCamera, const int minNrConstraintsPerLandmark) {
/* ************************************************************************* */
template<class GenericGraph>
int findSeparator(const GenericGraph& graph, const std::vector<size_t>& keys,
const int minNodesPerMap, WorkSpace& workspace, bool verbose,
const boost::optional<std::vector<Symbol> >& int2symbol, const bool reduceGraph,
const int minNrConstraintsPerCamera, const int minNrConstraintsPerLandmark) {
boost::optional<MetisResult> result = findPartitoning(graph, keys, workspace,
verbose, int2symbol, reduceGraph);
boost::optional<MetisResult> result = findPartitoning(graph, keys, workspace,
verbose, int2symbol, reduceGraph);
// find the island in A and B, and make them separated submaps
typedef std::vector<size_t> Island;
std::list<Island> islands;
// find the island in A and B, and make them separated submaps
typedef std::vector<size_t> Island;
std::list<Island> islands;
std::list<Island> islands_in_A = findIslands(graph, result->A, workspace,
minNrConstraintsPerCamera, minNrConstraintsPerLandmark);
std::list<Island> islands_in_A = findIslands(graph, result->A, workspace,
minNrConstraintsPerCamera, minNrConstraintsPerLandmark);
std::list<Island> islands_in_B = findIslands(graph, result->B, workspace,
minNrConstraintsPerCamera, minNrConstraintsPerLandmark);
std::list<Island> islands_in_B = findIslands(graph, result->B, workspace,
minNrConstraintsPerCamera, minNrConstraintsPerLandmark);
islands.insert(islands.end(), islands_in_A.begin(), islands_in_A.end());
islands.insert(islands.end(), islands_in_B.begin(), islands_in_B.end());
islands.sort(isLargerIsland);
size_t numIsland0 = islands.size();
islands.insert(islands.end(), islands_in_A.begin(), islands_in_A.end());
islands.insert(islands.end(), islands_in_B.begin(), islands_in_B.end());
islands.sort(isLargerIsland);
size_t numIsland0 = islands.size();
#ifdef NDEBUG
// verbose = true;
// if (!int2symbol) throw std::invalid_argument("findSeparator: int2symbol is not set!");
// std::cout << "sep size: " << result->C.size() << "; ";
// printNumCamerasLandmarks(result->C, *int2symbol);
// std::cout << "no. of island: " << islands.size() << "; ";
// std::cout << "island size: ";
// BOOST_FOREACH(const Island& island, islands)
// std::cout << island.size() << " ";
// std::cout << std::endl;
// verbose = true;
// if (!int2symbol) throw std::invalid_argument("findSeparator: int2symbol is not set!");
// std::cout << "sep size: " << result->C.size() << "; ";
// printNumCamerasLandmarks(result->C, *int2symbol);
// std::cout << "no. of island: " << islands.size() << "; ";
// std::cout << "island size: ";
// BOOST_FOREACH(const Island& island, islands)
// std::cout << island.size() << " ";
// std::cout << std::endl;
// BOOST_FOREACH(const Island& island, islands) {
// printNumCamerasLandmarks(island, int2symbol);
// }
// BOOST_FOREACH(const Island& island, islands) {
// printNumCamerasLandmarks(island, int2symbol);
// }
#endif
// absorb small components into the separator
size_t oldSize = islands.size();
while(true) {
if (islands.size() < 2) {
std::cout << "numIsland: " << numIsland0 << std::endl;
throw std::runtime_error("findSeparator: found fewer than 2 submaps!");
}
// absorb small components into the separator
size_t oldSize = islands.size();
while(true) {
if (islands.size() < 2) {
std::cout << "numIsland: " << numIsland0 << std::endl;
throw std::runtime_error("findSeparator: found fewer than 2 submaps!");
}
std::list<Island>::reference island = islands.back();
if ((int)island.size() >= minNodesPerMap) break;
result->C.insert(result->C.end(), island.begin(), island.end());
islands.pop_back();
}
if (islands.size() != oldSize){
if (verbose) std::cout << oldSize << "-" << oldSize - islands.size() << " submap(s);\t" << std::endl;
}
else{
if (verbose) std::cout << oldSize << " submap(s);\t" << std::endl;
}
std::list<Island>::reference island = islands.back();
if ((int)island.size() >= minNodesPerMap) break;
result->C.insert(result->C.end(), island.begin(), island.end());
islands.pop_back();
}
if (islands.size() != oldSize){
if (verbose) std::cout << oldSize << "-" << oldSize - islands.size() << " submap(s);\t" << std::endl;
}
else{
if (verbose) std::cout << oldSize << " submap(s);\t" << std::endl;
}
// generate the node map
std::vector<int>& partitionTable = workspace.partitionTable;
std::fill(partitionTable.begin(), partitionTable.end(), -1);
BOOST_FOREACH(const size_t key, result->C)
partitionTable[key] = 0;
int idx = 0;
BOOST_FOREACH(const Island& island, islands) {
idx++;
BOOST_FOREACH(const size_t key, island) {
partitionTable[key] = idx;
}
}
// generate the node map
std::vector<int>& partitionTable = workspace.partitionTable;
std::fill(partitionTable.begin(), partitionTable.end(), -1);
BOOST_FOREACH(const size_t key, result->C)
partitionTable[key] = 0;
int idx = 0;
BOOST_FOREACH(const Island& island, islands) {
idx++;
BOOST_FOREACH(const size_t key, island) {
partitionTable[key] = idx;
}
}
return islands.size();
}
return islands.size();
}
}} //namespace

42
gtsam_unstable/partition/FindSeparator.h
View File

@ -16,29 +16,29 @@
namespace gtsam { namespace partition {
// typedef std::map<size_t, size_t> PartitionTable; // from the key to the partition: 0 - separator, > 1: submap id
// typedef std::map<size_t, size_t> PartitionTable; // from the key to the partition: 0 - separator, > 1: submap id
/** the metis Nest dissection result */
struct MetisResult {
std::vector<size_t> A, B; // frontals
std::vector<size_t> C; // separator
};
/** the metis Nest dissection result */
struct MetisResult {
std::vector<size_t> A, B; // frontals
std::vector<size_t> C; // separator
};
/**
* use Metis library to partition, return the size of separator and the optional partition table
* the size of dictionary mush be equal to the number of variables in the original graph (the largest one)
*/
template<class GenericGraph>
boost::optional<MetisResult> separatorPartitionByMetis(const GenericGraph& graph, const std::vector<size_t>& keys,
WorkSpace& workspace, bool verbose);
/**
* use Metis library to partition, return the size of separator and the optional partition table
* the size of dictionary mush be equal to the number of variables in the original graph (the largest one)
*/
template<class GenericGraph>
boost::optional<MetisResult> separatorPartitionByMetis(const GenericGraph& graph, const std::vector<size_t>& keys,
WorkSpace& workspace, bool verbose);
/**
* return the number of submaps and the parition table of the partitioned graph (**stored in workspace.partitionTable**).
* return 0 if failed Note that the original output of Metis is 0,1 for submap, and 2 for the separator.
*/
template<class GenericGraph>
int findSeparator(const GenericGraph& graph, const std::vector<size_t>& keys,
const int minNodesPerMap, WorkSpace& workspace, bool verbose, const boost::optional<std::vector<Symbol> >& int2symbol,
const bool reduceGraph, const int minNrConstraintsPerCamera, const int minNrConstraintsPerLandmark);
/**
* return the number of submaps and the parition table of the partitioned graph (**stored in workspace.partitionTable**).
* return 0 if failed Note that the original output of Metis is 0,1 for submap, and 2 for the separator.
*/
template<class GenericGraph>
int findSeparator(const GenericGraph& graph, const std::vector<size_t>& keys,
const int minNodesPerMap, WorkSpace& workspace, bool verbose, const boost::optional<std::vector<Symbol> >& int2symbol,
const bool reduceGraph, const int minNrConstraintsPerCamera, const int minNrConstraintsPerLandmark);
}} //namespace

792
gtsam_unstable/partition/GenericGraph.cpp
View File

@ -19,459 +19,459 @@ using namespace std;
namespace gtsam { namespace partition {
/**
* Note: Need to be able to handle a graph with factors that involve variables not in the given {keys}
*/
list<vector<size_t> > findIslands(const GenericGraph2D& graph, const vector<size_t>& keys, WorkSpace& workspace,
const int minNrConstraintsPerCamera, const int minNrConstraintsPerLandmark)
{
typedef pair<int, int> IntPair;
typedef list<sharedGenericFactor2D> FactorList;
typedef map<IntPair, FactorList::iterator> Connections;
/**
* Note: Need to be able to handle a graph with factors that involve variables not in the given {keys}
*/
list<vector<size_t> > findIslands(const GenericGraph2D& graph, const vector<size_t>& keys, WorkSpace& workspace,
const int minNrConstraintsPerCamera, const int minNrConstraintsPerLandmark)
{
typedef pair<int, int> IntPair;
typedef list<sharedGenericFactor2D> FactorList;
typedef map<IntPair, FactorList::iterator> Connections;
// create disjoin set forest
DSFVector dsf(workspace.dsf, keys);
// create disjoin set forest
DSFVector dsf(workspace.dsf, keys);
FactorList factors(graph.begin(), graph.end());
size_t nrFactors = factors.size();
FactorList::iterator itEnd;
workspace.prepareDictionary(keys);
while (nrFactors) {
Connections connections;
bool succeed = false;
itEnd = factors.end();
list<FactorList::iterator> toErase;
for (FactorList::iterator itFactor=factors.begin(); itFactor!=itEnd; itFactor++) {
FactorList factors(graph.begin(), graph.end());
size_t nrFactors = factors.size();
FactorList::iterator itEnd;
workspace.prepareDictionary(keys);
while (nrFactors) {
Connections connections;
bool succeed = false;
itEnd = factors.end();
list<FactorList::iterator> toErase;
for (FactorList::iterator itFactor=factors.begin(); itFactor!=itEnd; itFactor++) {
// remove invalid factors
GenericNode2D key1 = (*itFactor)->key1, key2 = (*itFactor)->key2;
if (workspace.dictionary[key1.index]==-1 || workspace.dictionary[key2.index]==-1) {
toErase.push_back(itFactor); nrFactors--; continue;
}
// remove invalid factors
GenericNode2D key1 = (*itFactor)->key1, key2 = (*itFactor)->key2;
if (workspace.dictionary[key1.index]==-1 || workspace.dictionary[key2.index]==-1) {
toErase.push_back(itFactor); nrFactors--; continue;
}
size_t label1 = dsf.findSet(key1.index);
size_t label2 = dsf.findSet(key2.index);
if (label1 == label2) { toErase.push_back(itFactor); nrFactors--; continue; }
size_t label1 = dsf.findSet(key1.index);
size_t label2 = dsf.findSet(key2.index);
if (label1 == label2) { toErase.push_back(itFactor); nrFactors--; continue; }
// merge two trees if the connection is strong enough, otherwise cache it
// an odometry factor always merges two islands
if (key1.type == NODE_POSE_2D && key2.type == NODE_POSE_2D) {
toErase.push_back(itFactor); nrFactors--;
dsf.makeUnionInPlace(label1, label2);
succeed = true;
break;
}
// merge two trees if the connection is strong enough, otherwise cache it
// an odometry factor always merges two islands
if (key1.type == NODE_POSE_2D && key2.type == NODE_POSE_2D) {
toErase.push_back(itFactor); nrFactors--;
dsf.makeUnionInPlace(label1, label2);
succeed = true;
break;
}
// single landmark island only need one measurement
if ((dsf.isSingleton(label1)==1 && key1.type == NODE_LANDMARK_2D) ||
(dsf.isSingleton(label2)==1 && key2.type == NODE_LANDMARK_2D)) {
toErase.push_back(itFactor); nrFactors--;
dsf.makeUnionInPlace(label1, label2);
succeed = true;
break;
}
// single landmark island only need one measurement
if ((dsf.isSingleton(label1)==1 && key1.type == NODE_LANDMARK_2D) ||
(dsf.isSingleton(label2)==1 && key2.type == NODE_LANDMARK_2D)) {
toErase.push_back(itFactor); nrFactors--;
dsf.makeUnionInPlace(label1, label2);
succeed = true;
break;
}
// stack the current factor with the cached constraint
IntPair labels = (label1 < label2) ? make_pair(label1, label2) : make_pair(label2, label1);
Connections::iterator itCached = connections.find(labels);
if (itCached == connections.end()) {
connections.insert(make_pair(labels, itFactor));
continue;
} else {
GenericNode2D key21 = (*itCached->second)->key1, key22 = (*itCached->second)->key2;
// if observe the same landmark, we can not merge, abandon the current factor
if ((key1.index == key21.index && key1.type == NODE_LANDMARK_2D) ||
(key1.index == key22.index && key1.type == NODE_LANDMARK_2D) ||
(key2.index == key21.index && key2.type == NODE_LANDMARK_2D) ||
(key2.index == key22.index && key2.type == NODE_LANDMARK_2D)) {
toErase.push_back(itFactor); nrFactors--;
continue;
} else {
toErase.push_back(itFactor); nrFactors--;
toErase.push_back(itCached->second); nrFactors--;
dsf.makeUnionInPlace(label1, label2);
connections.erase(itCached);
succeed = true;
break;
}
}
}
// stack the current factor with the cached constraint
IntPair labels = (label1 < label2) ? make_pair(label1, label2) : make_pair(label2, label1);
Connections::iterator itCached = connections.find(labels);
if (itCached == connections.end()) {
connections.insert(make_pair(labels, itFactor));
continue;
} else {
GenericNode2D key21 = (*itCached->second)->key1, key22 = (*itCached->second)->key2;
// if observe the same landmark, we can not merge, abandon the current factor
if ((key1.index == key21.index && key1.type == NODE_LANDMARK_2D) ||
(key1.index == key22.index && key1.type == NODE_LANDMARK_2D) ||
(key2.index == key21.index && key2.type == NODE_LANDMARK_2D) ||
(key2.index == key22.index && key2.type == NODE_LANDMARK_2D)) {
toErase.push_back(itFactor); nrFactors--;
continue;
} else {
toErase.push_back(itFactor); nrFactors--;
toErase.push_back(itCached->second); nrFactors--;
dsf.makeUnionInPlace(label1, label2);
connections.erase(itCached);
succeed = true;
break;
}
}
}
// erase unused factors
BOOST_FOREACH(const FactorList::iterator& it, toErase)
factors.erase(it);
// erase unused factors
BOOST_FOREACH(const FactorList::iterator& it, toErase)
factors.erase(it);
if (!succeed) break;
}
if (!succeed) break;
}
list<vector<size_t> > islands;
map<size_t, vector<size_t> > arrays = dsf.arrays();
size_t key; vector<size_t> array;
BOOST_FOREACH(boost::tie(key, array), arrays)
islands.push_back(array);
return islands;
}
list<vector<size_t> > islands;
map<size_t, vector<size_t> > arrays = dsf.arrays();
size_t key; vector<size_t> array;
BOOST_FOREACH(boost::tie(key, array), arrays)
islands.push_back(array);
return islands;
}
/* ************************************************************************* */
void print(const GenericGraph2D& graph, const std::string name) {
cout << name << endl;
BOOST_FOREACH(const sharedGenericFactor2D& factor_, graph)
cout << factor_->key1.index << " " << factor_->key2.index << endl;
}
/* ************************************************************************* */
void print(const GenericGraph2D& graph, const std::string name) {
cout << name << endl;
BOOST_FOREACH(const sharedGenericFactor2D& factor_, graph)
cout << factor_->key1.index << " " << factor_->key2.index << endl;
}
/* ************************************************************************* */
void print(const GenericGraph3D& graph, const std::string name) {
cout << name << endl;
BOOST_FOREACH(const sharedGenericFactor3D& factor_, graph)
cout << factor_->key1.index << " " << factor_->key2.index << " (" <<
factor_->key1.type << ", " << factor_->key2.type <<")" << endl;
}
/* ************************************************************************* */
void print(const GenericGraph3D& graph, const std::string name) {
cout << name << endl;
BOOST_FOREACH(const sharedGenericFactor3D& factor_, graph)
cout << factor_->key1.index << " " << factor_->key2.index << " (" <<
factor_->key1.type << ", " << factor_->key2.type <<")" << endl;
}
/* ************************************************************************* */
// create disjoin set forest
DSFVector createDSF(const GenericGraph3D& graph, const vector<size_t>& keys, const WorkSpace& workspace) {
DSFVector dsf(workspace.dsf, keys);
typedef list<sharedGenericFactor3D> FactorList;
/* ************************************************************************* */
// create disjoin set forest
DSFVector createDSF(const GenericGraph3D& graph, const vector<size_t>& keys, const WorkSpace& workspace) {
DSFVector dsf(workspace.dsf, keys);
typedef list<sharedGenericFactor3D> FactorList;
FactorList factors(graph.begin(), graph.end());
size_t nrFactors = factors.size();
FactorList::iterator itEnd;
while (nrFactors) {
FactorList factors(graph.begin(), graph.end());
size_t nrFactors = factors.size();
FactorList::iterator itEnd;
while (nrFactors) {
bool succeed = false;
itEnd = factors.end();
list<FactorList::iterator> toErase;
for (FactorList::iterator itFactor=factors.begin(); itFactor!=itEnd; itFactor++) {
bool succeed = false;
itEnd = factors.end();
list<FactorList::iterator> toErase;
for (FactorList::iterator itFactor=factors.begin(); itFactor!=itEnd; itFactor++) {
// remove invalid factors
if (graph.size() == 178765) cout << "kai21" << endl;
GenericNode3D key1 = (*itFactor)->key1, key2 = (*itFactor)->key2;
if (graph.size() == 178765) cout << "kai21: " << key1.index << " " << key2.index << endl;
if (workspace.dictionary[key1.index]==-1 || workspace.dictionary[key2.index]==-1) {
toErase.push_back(itFactor); nrFactors--; continue;
}
// remove invalid factors
if (graph.size() == 178765) cout << "kai21" << endl;
GenericNode3D key1 = (*itFactor)->key1, key2 = (*itFactor)->key2;
if (graph.size() == 178765) cout << "kai21: " << key1.index << " " << key2.index << endl;
if (workspace.dictionary[key1.index]==-1 || workspace.dictionary[key2.index]==-1) {
toErase.push_back(itFactor); nrFactors--; continue;
}
if (graph.size() == 178765) cout << "kai22" << endl;
size_t label1 = dsf.findSet(key1.index);
size_t label2 = dsf.findSet(key2.index);
if (label1 == label2) { toErase.push_back(itFactor); nrFactors--; continue; }
if (graph.size() == 178765) cout << "kai22" << endl;
size_t label1 = dsf.findSet(key1.index);
size_t label2 = dsf.findSet(key2.index);
if (label1 == label2) { toErase.push_back(itFactor); nrFactors--; continue; }
if (graph.size() == 178765) cout << "kai23" << endl;
// merge two trees if the connection is strong enough, otherwise cache it
// an odometry factor always merges two islands
if ((key1.type == NODE_POSE_3D && key2.type == NODE_LANDMARK_3D) ||
(key1.type == NODE_POSE_3D && key2.type == NODE_POSE_3D)) {
toErase.push_back(itFactor); nrFactors--;
dsf.makeUnionInPlace(label1, label2);
succeed = true;
break;
}
if (graph.size() == 178765) cout << "kai23" << endl;
// merge two trees if the connection is strong enough, otherwise cache it
// an odometry factor always merges two islands
if ((key1.type == NODE_POSE_3D && key2.type == NODE_LANDMARK_3D) ||
(key1.type == NODE_POSE_3D && key2.type == NODE_POSE_3D)) {
toErase.push_back(itFactor); nrFactors--;
dsf.makeUnionInPlace(label1, label2);
succeed = true;
break;
}
if (graph.size() == 178765) cout << "kai24" << endl;
if (graph.size() == 178765) cout << "kai24" << endl;
}
}
// erase unused factors
BOOST_FOREACH(const FactorList::iterator& it, toErase)
factors.erase(it);
// erase unused factors
BOOST_FOREACH(const FactorList::iterator& it, toErase)
factors.erase(it);
if (!succeed) break;
}
return dsf;
}
if (!succeed) break;
}
return dsf;
}
/* ************************************************************************* */
// first check the type of the key (pose or landmark), and then check whether it is singular
inline bool isSingular(const set<size_t>& singularCameras, const set<size_t>& singularLandmarks, const GenericNode3D& node) {
switch(node.type) {
case NODE_POSE_3D:
return singularCameras.find(node.index) != singularCameras.end(); break;
case NODE_LANDMARK_3D:
return singularLandmarks.find(node.index) != singularLandmarks.end(); break;
default:
throw runtime_error("unrecognized key type!");
}
}
/* ************************************************************************* */
// first check the type of the key (pose or landmark), and then check whether it is singular
inline bool isSingular(const set<size_t>& singularCameras, const set<size_t>& singularLandmarks, const GenericNode3D& node) {
switch(node.type) {
case NODE_POSE_3D:
return singularCameras.find(node.index) != singularCameras.end(); break;
case NODE_LANDMARK_3D:
return singularLandmarks.find(node.index) != singularLandmarks.end(); break;
default:
throw runtime_error("unrecognized key type!");
}
}
/* ************************************************************************* */
void findSingularCamerasLandmarks(const GenericGraph3D& graph, const WorkSpace& workspace,
const vector<bool>& isCamera, const vector<bool>& isLandmark,
set<size_t>& singularCameras, set<size_t>& singularLandmarks, vector<int>& nrConstraints,
bool& foundSingularCamera, bool& foundSingularLandmark,
const int minNrConstraintsPerCamera, const int minNrConstraintsPerLandmark) {
/* ************************************************************************* */
void findSingularCamerasLandmarks(const GenericGraph3D& graph, const WorkSpace& workspace,
const vector<bool>& isCamera, const vector<bool>& isLandmark,
set<size_t>& singularCameras, set<size_t>& singularLandmarks, vector<int>& nrConstraints,
bool& foundSingularCamera, bool& foundSingularLandmark,
const int minNrConstraintsPerCamera, const int minNrConstraintsPerLandmark) {
// compute the constraint number per camera
std::fill(nrConstraints.begin(), nrConstraints.end(), 0);
BOOST_FOREACH(const sharedGenericFactor3D& factor_, graph) {
const int& key1 = factor_->key1.index;
const int& key2 = factor_->key2.index;
if (workspace.dictionary[key1] != -1 && workspace.dictionary[key2] != -1 &&
!isSingular(singularCameras, singularLandmarks, factor_->key1) &&
!isSingular(singularCameras, singularLandmarks, factor_->key2)) {
nrConstraints[key1]++;
nrConstraints[key2]++;
// compute the constraint number per camera
std::fill(nrConstraints.begin(), nrConstraints.end(), 0);
BOOST_FOREACH(const sharedGenericFactor3D& factor_, graph) {
const int& key1 = factor_->key1.index;
const int& key2 = factor_->key2.index;
if (workspace.dictionary[key1] != -1 && workspace.dictionary[key2] != -1 &&
!isSingular(singularCameras, singularLandmarks, factor_->key1) &&
!isSingular(singularCameras, singularLandmarks, factor_->key2)) {
nrConstraints[key1]++;
nrConstraints[key2]++;
// a single pose constraint is sufficient for stereo, so we add 2 to the counter
// for a total of 3, i.e. the same as 3 landmarks fully constraining the camera
if(factor_->key1.type == NODE_POSE_3D && factor_->key2.type == NODE_POSE_3D){
nrConstraints[key1]+=2;
nrConstraints[key2]+=2;
}
}
}
// a single pose constraint is sufficient for stereo, so we add 2 to the counter
// for a total of 3, i.e. the same as 3 landmarks fully constraining the camera
if(factor_->key1.type == NODE_POSE_3D && factor_->key2.type == NODE_POSE_3D){
nrConstraints[key1]+=2;
nrConstraints[key2]+=2;
}
}
}
// find singular cameras and landmarks
foundSingularCamera = false;
foundSingularLandmark = false;
for (size_t i=0; i<nrConstraints.size(); i++) {
if (isCamera[i] && nrConstraints[i] < minNrConstraintsPerCamera &&
singularCameras.find(i) == singularCameras.end()) {
singularCameras.insert(i);
foundSingularCamera = true;
}
if (isLandmark[i] && nrConstraints[i] < minNrConstraintsPerLandmark &&
singularLandmarks.find(i) == singularLandmarks.end()) {
singularLandmarks.insert(i);
foundSingularLandmark = true;
}
}
}
// find singular cameras and landmarks
foundSingularCamera = false;
foundSingularLandmark = false;
for (size_t i=0; i<nrConstraints.size(); i++) {
if (isCamera[i] && nrConstraints[i] < minNrConstraintsPerCamera &&
singularCameras.find(i) == singularCameras.end()) {
singularCameras.insert(i);
foundSingularCamera = true;
}
if (isLandmark[i] && nrConstraints[i] < minNrConstraintsPerLandmark &&
singularLandmarks.find(i) == singularLandmarks.end()) {
singularLandmarks.insert(i);
foundSingularLandmark = true;
}
}
}
/* ************************************************************************* */
list<vector<size_t> > findIslands(const GenericGraph3D& graph, const vector<size_t>& keys, WorkSpace& workspace,
const size_t minNrConstraintsPerCamera, const size_t minNrConstraintsPerLandmark) {
/* ************************************************************************* */
list<vector<size_t> > findIslands(const GenericGraph3D& graph, const vector<size_t>& keys, WorkSpace& workspace,
const size_t minNrConstraintsPerCamera, const size_t minNrConstraintsPerLandmark) {
// create disjoint set forest
workspace.prepareDictionary(keys);
DSFVector dsf = createDSF(graph, keys, workspace);
// create disjoint set forest
workspace.prepareDictionary(keys);
DSFVector dsf = createDSF(graph, keys, workspace);
const bool verbose = false;
bool foundSingularCamera = true;
bool foundSingularLandmark = true;
const bool verbose = false;
bool foundSingularCamera = true;
bool foundSingularLandmark = true;
list<vector<size_t> > islands;
set<size_t> singularCameras, singularLandmarks;
vector<bool> isCamera(workspace.dictionary.size(), false);
vector<bool> isLandmark(workspace.dictionary.size(), false);
list<vector<size_t> > islands;
set<size_t> singularCameras, singularLandmarks;
vector<bool> isCamera(workspace.dictionary.size(), false);
vector<bool> isLandmark(workspace.dictionary.size(), false);
// check the constraint number of every variable
// find the camera and landmark keys
BOOST_FOREACH(const sharedGenericFactor3D& factor_, graph) {
//assert(factor_->key2.type == NODE_LANDMARK_3D); // only VisualSLAM should come here, not StereoSLAM
if (workspace.dictionary[factor_->key1.index] != -1) {
if (factor_->key1.type == NODE_POSE_3D)
isCamera[factor_->key1.index] = true;
else
isLandmark[factor_->key1.index] = true;
}
// check the constraint number of every variable
// find the camera and landmark keys
BOOST_FOREACH(const sharedGenericFactor3D& factor_, graph) {
//assert(factor_->key2.type == NODE_LANDMARK_3D); // only VisualSLAM should come here, not StereoSLAM
if (workspace.dictionary[factor_->key1.index] != -1) {
if (factor_->key1.type == NODE_POSE_3D)
isCamera[factor_->key1.index] = true;
else
isLandmark[factor_->key1.index] = true;
}
if (workspace.dictionary[factor_->key2.index] != -1) {
if (factor_->key2.type == NODE_POSE_3D)
isCamera[factor_->key2.index] = true;
else
isLandmark[factor_->key2.index] = true;
if (factor_->key2.type == NODE_POSE_3D)
isCamera[factor_->key2.index] = true;
else
isLandmark[factor_->key2.index] = true;
}
}
}
vector<int> nrConstraints(workspace.dictionary.size(), 0);
// iterate until all singular variables have been removed. Removing a singular variable
// can cause another to become singular, so this will probably run several times
while (foundSingularCamera || foundSingularLandmark) {
findSingularCamerasLandmarks(graph, workspace, isCamera, isLandmark, // input
singularCameras, singularLandmarks, nrConstraints, // output
foundSingularCamera, foundSingularLandmark, // output
minNrConstraintsPerCamera, minNrConstraintsPerLandmark); // input
}
vector<int> nrConstraints(workspace.dictionary.size(), 0);
// iterate until all singular variables have been removed. Removing a singular variable
// can cause another to become singular, so this will probably run several times
while (foundSingularCamera || foundSingularLandmark) {
findSingularCamerasLandmarks(graph, workspace, isCamera, isLandmark, // input
singularCameras, singularLandmarks, nrConstraints, // output
foundSingularCamera, foundSingularLandmark, // output
minNrConstraintsPerCamera, minNrConstraintsPerLandmark); // input
}
// add singular variables directly as islands
if (!singularCameras.empty()) {
if (verbose) cout << "singular cameras:";
BOOST_FOREACH(const size_t i, singularCameras) {
islands.push_back(vector<size_t>(1, i)); // <---------------------------
if (verbose) cout << i << " ";
}
if (verbose) cout << endl;
}
if (!singularLandmarks.empty()) {
if (verbose) cout << "singular landmarks:";
BOOST_FOREACH(const size_t i, singularLandmarks) {
islands.push_back(vector<size_t>(1, i)); // <---------------------------
if (verbose) cout << i << " ";
}
if (verbose) cout << endl;
}
// add singular variables directly as islands
if (!singularCameras.empty()) {
if (verbose) cout << "singular cameras:";
BOOST_FOREACH(const size_t i, singularCameras) {
islands.push_back(vector<size_t>(1, i)); // <---------------------------
if (verbose) cout << i << " ";
}
if (verbose) cout << endl;
}
if (!singularLandmarks.empty()) {
if (verbose) cout << "singular landmarks:";
BOOST_FOREACH(const size_t i, singularLandmarks) {
islands.push_back(vector<size_t>(1, i)); // <---------------------------
if (verbose) cout << i << " ";
}
if (verbose) cout << endl;
}
// regenerating islands
map<size_t, vector<size_t> > labelIslands = dsf.arrays();
size_t label; vector<size_t> island;
BOOST_FOREACH(boost::tie(label, island), labelIslands) {
vector<size_t> filteredIsland; // remove singular cameras from array
filteredIsland.reserve(island.size());
BOOST_FOREACH(const size_t key, island) {
if ((isCamera[key] && singularCameras.find(key) == singularCameras.end()) || // not singular
(isLandmark[key] && singularLandmarks.find(key) == singularLandmarks.end()) || // not singular
(!isCamera[key] && !isLandmark[key])) { // the key is not involved in any factor, so the type is undertermined
filteredIsland.push_back(key);
}
}
islands.push_back(filteredIsland);
}
// regenerating islands
map<size_t, vector<size_t> > labelIslands = dsf.arrays();
size_t label; vector<size_t> island;
BOOST_FOREACH(boost::tie(label, island), labelIslands) {
vector<size_t> filteredIsland; // remove singular cameras from array
filteredIsland.reserve(island.size());
BOOST_FOREACH(const size_t key, island) {
if ((isCamera[key] && singularCameras.find(key) == singularCameras.end()) || // not singular
(isLandmark[key] && singularLandmarks.find(key) == singularLandmarks.end()) || // not singular
(!isCamera[key] && !isLandmark[key])) { // the key is not involved in any factor, so the type is undertermined
filteredIsland.push_back(key);
}
}
islands.push_back(filteredIsland);
}
// sanity check
size_t nrKeys = 0;
BOOST_FOREACH(const vector<size_t>& island, islands)
nrKeys += island.size();
if (nrKeys != keys.size()) {
cout << nrKeys << " vs " << keys.size() << endl;
throw runtime_error("findIslands: the number of keys is inconsistent!");
}
// sanity check
size_t nrKeys = 0;
BOOST_FOREACH(const vector<size_t>& island, islands)
nrKeys += island.size();
if (nrKeys != keys.size()) {
cout << nrKeys << " vs " << keys.size() << endl;
throw runtime_error("findIslands: the number of keys is inconsistent!");
}
if (verbose) cout << "found " << islands.size() << " islands!" << endl;
return islands;
}
if (verbose) cout << "found " << islands.size() << " islands!" << endl;
return islands;
}
/* ************************************************************************* */
// return the number of intersection between two **sorted** landmark vectors
inline int getNrCommonLandmarks(const vector<size_t>& landmarks1, const vector<size_t>& landmarks2){
size_t i1 = 0, i2 = 0;
int nrCommonLandmarks = 0;
while (i1 < landmarks1.size() && i2 < landmarks2.size()) {
if (landmarks1[i1] < landmarks2[i2])
i1 ++;
else if (landmarks1[i1] > landmarks2[i2])
i2 ++;
else {
i1++; i2++;
nrCommonLandmarks ++;
}
}
return nrCommonLandmarks;
}
/* ************************************************************************* */
// return the number of intersection between two **sorted** landmark vectors
inline int getNrCommonLandmarks(const vector<size_t>& landmarks1, const vector<size_t>& landmarks2){
size_t i1 = 0, i2 = 0;
int nrCommonLandmarks = 0;
while (i1 < landmarks1.size() && i2 < landmarks2.size()) {
if (landmarks1[i1] < landmarks2[i2])
i1 ++;
else if (landmarks1[i1] > landmarks2[i2])
i2 ++;
else {
i1++; i2++;
nrCommonLandmarks ++;
}
}
return nrCommonLandmarks;
}
/* ************************************************************************* */
void reduceGenericGraph(const GenericGraph3D& graph, const std::vector<size_t>& cameraKeys, const std::vector<size_t>& landmarkKeys,
const std::vector<int>& dictionary, GenericGraph3D& reducedGraph) {
/* ************************************************************************* */
void reduceGenericGraph(const GenericGraph3D& graph, const std::vector<size_t>& cameraKeys, const std::vector<size_t>& landmarkKeys,
const std::vector<int>& dictionary, GenericGraph3D& reducedGraph) {
typedef size_t CameraKey;
typedef pair<CameraKey, CameraKey> CameraPair;
typedef size_t LandmarkKey;
// get a mapping from each landmark to its connected cameras
vector<vector<LandmarkKey> > cameraToLandmarks(dictionary.size());
// for odometry xi-xj where i<j, we always store cameraToCamera[i] = j, otherwise equal to -1 if no odometry
vector<int> cameraToCamera(dictionary.size(), -1);
size_t key_i, key_j;
BOOST_FOREACH(const sharedGenericFactor3D& factor_, graph) {
if (factor_->key1.type == NODE_POSE_3D) {
if (factor_->key2.type == NODE_LANDMARK_3D) {// projection factor
cameraToLandmarks[factor_->key1.index].push_back(factor_->key2.index);
}
else { // odometry factor
if (factor_->key1.index < factor_->key2.index) {
key_i = factor_->key1.index;
key_j = factor_->key2.index;
} else {
key_i = factor_->key2.index;
key_j = factor_->key1.index;
}
cameraToCamera[key_i] = key_j;
}
}
}
typedef size_t CameraKey;
typedef pair<CameraKey, CameraKey> CameraPair;
typedef size_t LandmarkKey;
// get a mapping from each landmark to its connected cameras
vector<vector<LandmarkKey> > cameraToLandmarks(dictionary.size());
// for odometry xi-xj where i<j, we always store cameraToCamera[i] = j, otherwise equal to -1 if no odometry
vector<int> cameraToCamera(dictionary.size(), -1);
size_t key_i, key_j;
BOOST_FOREACH(const sharedGenericFactor3D& factor_, graph) {
if (factor_->key1.type == NODE_POSE_3D) {
if (factor_->key2.type == NODE_LANDMARK_3D) {// projection factor
cameraToLandmarks[factor_->key1.index].push_back(factor_->key2.index);
}
else { // odometry factor
if (factor_->key1.index < factor_->key2.index) {
key_i = factor_->key1.index;
key_j = factor_->key2.index;
} else {
key_i = factor_->key2.index;
key_j = factor_->key1.index;
}
cameraToCamera[key_i] = key_j;
}
}
}
// sort the landmark keys for the late getNrCommonLandmarks call
BOOST_FOREACH(vector<LandmarkKey> &landmarks, cameraToLandmarks){
if (!landmarks.empty())
std::sort(landmarks.begin(), landmarks.end());
}
// sort the landmark keys for the late getNrCommonLandmarks call
BOOST_FOREACH(vector<LandmarkKey> &landmarks, cameraToLandmarks){
if (!landmarks.empty())
std::sort(landmarks.begin(), landmarks.end());
}
// generate the reduced graph
reducedGraph.clear();
int factorIndex = 0;
int camera1, camera2, nrTotalConstraints;
bool hasOdometry;
for (size_t i1=0; i1<cameraKeys.size()-1; ++i1) {
for (size_t i2=i1+1; i2<cameraKeys.size(); ++i2) {
camera1 = cameraKeys[i1];
camera2 = cameraKeys[i2];
int nrCommonLandmarks = getNrCommonLandmarks(cameraToLandmarks[camera1], cameraToLandmarks[camera2]);
hasOdometry = cameraToCamera[camera1] == camera2;
if (nrCommonLandmarks > 0 || hasOdometry) {
nrTotalConstraints = 2 * nrCommonLandmarks + (hasOdometry ? 6 : 0);
reducedGraph.push_back(boost::make_shared<GenericFactor3D>(camera1, camera2,
factorIndex++, NODE_POSE_3D, NODE_POSE_3D, nrTotalConstraints));
}
}
}
}
// generate the reduced graph
reducedGraph.clear();
int factorIndex = 0;
int camera1, camera2, nrTotalConstraints;
bool hasOdometry;
for (size_t i1=0; i1<cameraKeys.size()-1; ++i1) {
for (size_t i2=i1+1; i2<cameraKeys.size(); ++i2) {
camera1 = cameraKeys[i1];
camera2 = cameraKeys[i2];
int nrCommonLandmarks = getNrCommonLandmarks(cameraToLandmarks[camera1], cameraToLandmarks[camera2]);
hasOdometry = cameraToCamera[camera1] == camera2;
if (nrCommonLandmarks > 0 || hasOdometry) {
nrTotalConstraints = 2 * nrCommonLandmarks + (hasOdometry ? 6 : 0);
reducedGraph.push_back(boost::make_shared<GenericFactor3D>(camera1, camera2,
factorIndex++, NODE_POSE_3D, NODE_POSE_3D, nrTotalConstraints));
}
}
}
}
/* ************************************************************************* */
void checkSingularity(const GenericGraph3D& graph, const std::vector<size_t>& frontals,
WorkSpace& workspace, const size_t minNrConstraintsPerCamera, const size_t minNrConstraintsPerLandmark) {
workspace.prepareDictionary(frontals);
vector<size_t> nrConstraints(workspace.dictionary.size(), 0);
/* ************************************************************************* */
void checkSingularity(const GenericGraph3D& graph, const std::vector<size_t>& frontals,
WorkSpace& workspace, const size_t minNrConstraintsPerCamera, const size_t minNrConstraintsPerLandmark) {
workspace.prepareDictionary(frontals);
vector<size_t> nrConstraints(workspace.dictionary.size(), 0);
// summarize the constraint number
const vector<int>& dictionary = workspace.dictionary;
vector<bool> isValidCamera(workspace.dictionary.size(), false);
vector<bool> isValidLandmark(workspace.dictionary.size(), false);
BOOST_FOREACH(const sharedGenericFactor3D& factor_, graph) {
assert(factor_->key1.type == NODE_POSE_3D);
//assert(factor_->key2.type == NODE_LANDMARK_3D);
const size_t& key1 = factor_->key1.index;
const size_t& key2 = factor_->key2.index;
if (dictionary[key1] == -1 || dictionary[key2] == -1)
continue;
// summarize the constraint number
const vector<int>& dictionary = workspace.dictionary;
vector<bool> isValidCamera(workspace.dictionary.size(), false);
vector<bool> isValidLandmark(workspace.dictionary.size(), false);
BOOST_FOREACH(const sharedGenericFactor3D& factor_, graph) {
assert(factor_->key1.type == NODE_POSE_3D);
//assert(factor_->key2.type == NODE_LANDMARK_3D);
const size_t& key1 = factor_->key1.index;
const size_t& key2 = factor_->key2.index;
if (dictionary[key1] == -1 || dictionary[key2] == -1)
continue;
isValidCamera[key1] = true;
if(factor_->key2.type == NODE_LANDMARK_3D)
isValidLandmark[key2] = true;
else
isValidCamera[key2] = true;
isValidCamera[key1] = true;
if(factor_->key2.type == NODE_LANDMARK_3D)
isValidLandmark[key2] = true;
else
isValidCamera[key2] = true;
nrConstraints[key1]++;
nrConstraints[key2]++;
nrConstraints[key1]++;
nrConstraints[key2]++;
// a single pose constraint is sufficient for stereo, so we add 2 to the counter
// for a total of 3, i.e. the same as 3 landmarks fully constraining the camera
if(factor_->key1.type == NODE_POSE_3D && factor_->key2.type == NODE_POSE_3D){
nrConstraints[key1]+=2;
nrConstraints[key2]+=2;
}
}
// a single pose constraint is sufficient for stereo, so we add 2 to the counter
// for a total of 3, i.e. the same as 3 landmarks fully constraining the camera
if(factor_->key1.type == NODE_POSE_3D && factor_->key2.type == NODE_POSE_3D){
nrConstraints[key1]+=2;
nrConstraints[key2]+=2;
}
}
// find the minimum constraint for cameras and landmarks
size_t minFoundConstraintsPerCamera = 10000;
size_t minFoundConstraintsPerLandmark = 10000;
// find the minimum constraint for cameras and landmarks
size_t minFoundConstraintsPerCamera = 10000;
size_t minFoundConstraintsPerLandmark = 10000;
for (size_t i=0; i<isValidCamera.size(); i++) {
if (isValidCamera[i]) {
minFoundConstraintsPerCamera = std::min(nrConstraints[i], minFoundConstraintsPerCamera);
if (nrConstraints[i] < minNrConstraintsPerCamera)
cout << "!!!!!!!!!!!!!!!!!!! camera with " << nrConstraints[i] << " constraint: " << i << endl;
}
for (size_t i=0; i<isValidCamera.size(); i++) {
if (isValidCamera[i]) {
minFoundConstraintsPerCamera = std::min(nrConstraints[i], minFoundConstraintsPerCamera);
if (nrConstraints[i] < minNrConstraintsPerCamera)
cout << "!!!!!!!!!!!!!!!!!!! camera with " << nrConstraints[i] << " constraint: " << i << endl;
}
}
for (size_t j=0; j<isValidLandmark.size(); j++) {
if (isValidLandmark[j]) {
minFoundConstraintsPerLandmark = std::min(nrConstraints[j], minFoundConstraintsPerLandmark);
if (nrConstraints[j] < minNrConstraintsPerLandmark)
cout << "!!!!!!!!!!!!!!!!!!! landmark with " << nrConstraints[j] << " constraint: " << j << endl;
}
}
}
for (size_t j=0; j<isValidLandmark.size(); j++) {
if (isValidLandmark[j]) {
minFoundConstraintsPerLandmark = std::min(nrConstraints[j], minFoundConstraintsPerLandmark);
if (nrConstraints[j] < minNrConstraintsPerLandmark)
cout << "!!!!!!!!!!!!!!!!!!! landmark with " << nrConstraints[j] << " constraint: " << j << endl;
}
}
// debug info
BOOST_FOREACH(const size_t key, frontals) {
if (isValidCamera[key] && nrConstraints[key] < minNrConstraintsPerCamera)
cout << "singular camera:" << key << " with " << nrConstraints[key] << " constraints" << endl;
}
// debug info
BOOST_FOREACH(const size_t key, frontals) {
if (isValidCamera[key] && nrConstraints[key] < minNrConstraintsPerCamera)
cout << "singular camera:" << key << " with " << nrConstraints[key] << " constraints" << endl;
}
if (minFoundConstraintsPerCamera < minNrConstraintsPerCamera)
throw runtime_error("checkSingularity:minConstraintsPerCamera < " + boost::lexical_cast<string>(minFoundConstraintsPerCamera));
if (minFoundConstraintsPerLandmark < minNrConstraintsPerLandmark)
throw runtime_error("checkSingularity:minConstraintsPerLandmark < " + boost::lexical_cast<string>(minFoundConstraintsPerLandmark));
}
if (minFoundConstraintsPerCamera < minNrConstraintsPerCamera)
throw runtime_error("checkSingularity:minConstraintsPerCamera < " + boost::lexical_cast<string>(minFoundConstraintsPerCamera));
if (minFoundConstraintsPerLandmark < minNrConstraintsPerLandmark)
throw runtime_error("checkSingularity:minConstraintsPerLandmark < " + boost::lexical_cast<string>(minFoundConstraintsPerLandmark));
}
}} // namespace

216
gtsam_unstable/partition/GenericGraph.h
View File

@ -17,133 +17,133 @@
namespace gtsam { namespace partition {
/***************************************************
* 2D generic factors and their factor graph
***************************************************/
enum GenericNode2DType { NODE_POSE_2D, NODE_LANDMARK_2D };
/***************************************************
* 2D generic factors and their factor graph
***************************************************/
enum GenericNode2DType { NODE_POSE_2D, NODE_LANDMARK_2D };
/** the index of the node and the type of the node */
struct GenericNode2D {
std::size_t index;
GenericNode2DType type;
GenericNode2D (const std::size_t& index_in, const GenericNode2DType& type_in) : index(index_in), type(type_in) {}
};
/** the index of the node and the type of the node */
struct GenericNode2D {
std::size_t index;
GenericNode2DType type;
GenericNode2D (const std::size_t& index_in, const GenericNode2DType& type_in) : index(index_in), type(type_in) {}
};
/** a factor always involves two nodes/variables for now */
struct GenericFactor2D {
GenericNode2D key1;
GenericNode2D key2;
int index; // the factor index in the original nonlinear factor graph
int weight; // the weight of the edge
GenericFactor2D(const size_t index1, const GenericNode2DType type1, const size_t index2, const GenericNode2DType type2, const int index_ = -1, const int weight_ = 1)
: key1(index1, type1), key2(index2, type2), index(index_), weight(weight_) {}
GenericFactor2D(const size_t index1, const char type1, const size_t index2, const char type2, const int index_ = -1, const int weight_ = 1)
: key1(index1, type1 == 'x' ? NODE_POSE_2D : NODE_LANDMARK_2D),
key2(index2, type2 == 'x' ? NODE_POSE_2D : NODE_LANDMARK_2D), index(index_), weight(weight_) {}
};
/** a factor always involves two nodes/variables for now */
struct GenericFactor2D {
GenericNode2D key1;
GenericNode2D key2;
int index; // the factor index in the original nonlinear factor graph
int weight; // the weight of the edge
GenericFactor2D(const size_t index1, const GenericNode2DType type1, const size_t index2, const GenericNode2DType type2, const int index_ = -1, const int weight_ = 1)
: key1(index1, type1), key2(index2, type2), index(index_), weight(weight_) {}
GenericFactor2D(const size_t index1, const char type1, const size_t index2, const char type2, const int index_ = -1, const int weight_ = 1)
: key1(index1, type1 == 'x' ? NODE_POSE_2D : NODE_LANDMARK_2D),
key2(index2, type2 == 'x' ? NODE_POSE_2D : NODE_LANDMARK_2D), index(index_), weight(weight_) {}
};
/** graph is a collection of factors */
typedef boost::shared_ptr<GenericFactor2D> sharedGenericFactor2D;
typedef std::vector<sharedGenericFactor2D> GenericGraph2D;
/** graph is a collection of factors */
typedef boost::shared_ptr<GenericFactor2D> sharedGenericFactor2D;
typedef std::vector<sharedGenericFactor2D> GenericGraph2D;
/** merge nodes in DSF using constraints captured by the given graph */
std::list<std::vector<size_t> > findIslands(const GenericGraph2D& graph, const std::vector<size_t>& keys, WorkSpace& workspace,
const int minNrConstraintsPerCamera, const int minNrConstraintsPerLandmark);
/** merge nodes in DSF using constraints captured by the given graph */
std::list<std::vector<size_t> > findIslands(const GenericGraph2D& graph, const std::vector<size_t>& keys, WorkSpace& workspace,
const int minNrConstraintsPerCamera, const int minNrConstraintsPerLandmark);
/** eliminate the sensors from generic graph */
inline void reduceGenericGraph(const GenericGraph2D& graph, const std::vector<size_t>& cameraKeys, const std::vector<size_t>& landmarkKeys,
const std::vector<int>& dictionary, GenericGraph2D& reducedGraph) {
throw std::runtime_error("reduceGenericGraph 2d not implemented");
}
/** eliminate the sensors from generic graph */
inline void reduceGenericGraph(const GenericGraph2D& graph, const std::vector<size_t>& cameraKeys, const std::vector<size_t>& landmarkKeys,
const std::vector<int>& dictionary, GenericGraph2D& reducedGraph) {
throw std::runtime_error("reduceGenericGraph 2d not implemented");
}
/** check whether the 2D graph is singular (under constrained) , Dummy function for 2D */
inline void checkSingularity(const GenericGraph2D& graph, const std::vector<size_t>& frontals,
WorkSpace& workspace, const int minNrConstraintsPerCamera, const int minNrConstraintsPerLandmark) { return; }
/** check whether the 2D graph is singular (under constrained) , Dummy function for 2D */
inline void checkSingularity(const GenericGraph2D& graph, const std::vector<size_t>& frontals,
WorkSpace& workspace, const int minNrConstraintsPerCamera, const int minNrConstraintsPerLandmark) { return; }
/** print the graph **/
void print(const GenericGraph2D& graph, const std::string name = "GenericGraph2D");
/** print the graph **/
void print(const GenericGraph2D& graph, const std::string name = "GenericGraph2D");
/***************************************************
* 3D generic factors and their factor graph
***************************************************/
enum GenericNode3DType { NODE_POSE_3D, NODE_LANDMARK_3D };
/***************************************************
* 3D generic factors and their factor graph
***************************************************/
enum GenericNode3DType { NODE_POSE_3D, NODE_LANDMARK_3D };
// const int minNrConstraintsPerCamera = 7;
// const int minNrConstraintsPerLandmark = 2;
// const int minNrConstraintsPerCamera = 7;
// const int minNrConstraintsPerLandmark = 2;
/** the index of the node and the type of the node */
struct GenericNode3D {
std::size_t index;
GenericNode3DType type;
GenericNode3D (const std::size_t& index_in, const GenericNode3DType& type_in) : index(index_in), type(type_in) {}
};
/** the index of the node and the type of the node */
struct GenericNode3D {
std::size_t index;
GenericNode3DType type;
GenericNode3D (const std::size_t& index_in, const GenericNode3DType& type_in) : index(index_in), type(type_in) {}
};
/** a factor always involves two nodes/variables for now */
struct GenericFactor3D {
GenericNode3D key1;
GenericNode3D key2;
int index; // the index in the entire graph, 0-based
int weight; // the weight of the edge
GenericFactor3D() :key1(-1, NODE_POSE_3D), key2(-1, NODE_LANDMARK_3D), index(-1), weight(1) {}
GenericFactor3D(const size_t index1, const size_t index2, const int index_ = -1,
const GenericNode3DType type1 = NODE_POSE_3D, const GenericNode3DType type2 = NODE_LANDMARK_3D, const int weight_ = 1)
: key1(index1, type1), key2(index2, type2), index(index_), weight(weight_) {}
};
/** a factor always involves two nodes/variables for now */
struct GenericFactor3D {
GenericNode3D key1;
GenericNode3D key2;
int index; // the index in the entire graph, 0-based
int weight; // the weight of the edge
GenericFactor3D() :key1(-1, NODE_POSE_3D), key2(-1, NODE_LANDMARK_3D), index(-1), weight(1) {}
GenericFactor3D(const size_t index1, const size_t index2, const int index_ = -1,
const GenericNode3DType type1 = NODE_POSE_3D, const GenericNode3DType type2 = NODE_LANDMARK_3D, const int weight_ = 1)
: key1(index1, type1), key2(index2, type2), index(index_), weight(weight_) {}
};
/** graph is a collection of factors */
typedef boost::shared_ptr<GenericFactor3D> sharedGenericFactor3D;
typedef std::vector<sharedGenericFactor3D> GenericGraph3D;
/** graph is a collection of factors */
typedef boost::shared_ptr<GenericFactor3D> sharedGenericFactor3D;
typedef std::vector<sharedGenericFactor3D> GenericGraph3D;
/** merge nodes in DSF using constraints captured by the given graph */
std::list<std::vector<size_t> > findIslands(const GenericGraph3D& graph, const std::vector<size_t>& keys, WorkSpace& workspace,
const size_t minNrConstraintsPerCamera, const size_t minNrConstraintsPerLandmark);
/** merge nodes in DSF using constraints captured by the given graph */
std::list<std::vector<size_t> > findIslands(const GenericGraph3D& graph, const std::vector<size_t>& keys, WorkSpace& workspace,
const size_t minNrConstraintsPerCamera, const size_t minNrConstraintsPerLandmark);
/** eliminate the sensors from generic graph */
void reduceGenericGraph(const GenericGraph3D& graph, const std::vector<size_t>& cameraKeys, const std::vector<size_t>& landmarkKeys,
const std::vector<int>& dictionary, GenericGraph3D& reducedGraph);
/** eliminate the sensors from generic graph */
void reduceGenericGraph(const GenericGraph3D& graph, const std::vector<size_t>& cameraKeys, const std::vector<size_t>& landmarkKeys,
const std::vector<int>& dictionary, GenericGraph3D& reducedGraph);
/** check whether the 3D graph is singular (under constrained) */
void checkSingularity(const GenericGraph3D& graph, const std::vector<size_t>& frontals,
WorkSpace& workspace, const size_t minNrConstraintsPerCamera, const size_t minNrConstraintsPerLandmark);
/** check whether the 3D graph is singular (under constrained) */
void checkSingularity(const GenericGraph3D& graph, const std::vector<size_t>& frontals,
WorkSpace& workspace, const size_t minNrConstraintsPerCamera, const size_t minNrConstraintsPerLandmark);
/** print the graph **/
void print(const GenericGraph3D& graph, const std::string name = "GenericGraph3D");
/** print the graph **/
void print(const GenericGraph3D& graph, const std::string name = "GenericGraph3D");
/***************************************************
* unary generic factors and their factor graph
***************************************************/
/** a factor involves a single variable */
struct GenericUnaryFactor {
GenericNode2D key;
int index; // the factor index in the original nonlinear factor graph
GenericUnaryFactor(const size_t key_, const GenericNode2DType type_, const int index_ = -1)
: key(key_, type_), index(index_) {}
GenericUnaryFactor(const size_t key_, const char type_, const int index_ = -1)
: key(key_, type_ == 'x' ? NODE_POSE_2D : NODE_LANDMARK_2D), index(index_) {}
};
/***************************************************
* unary generic factors and their factor graph
***************************************************/
/** a factor involves a single variable */
struct GenericUnaryFactor {
GenericNode2D key;
int index; // the factor index in the original nonlinear factor graph
GenericUnaryFactor(const size_t key_, const GenericNode2DType type_, const int index_ = -1)
: key(key_, type_), index(index_) {}
GenericUnaryFactor(const size_t key_, const char type_, const int index_ = -1)
: key(key_, type_ == 'x' ? NODE_POSE_2D : NODE_LANDMARK_2D), index(index_) {}
};
/** graph is a collection of factors */
typedef boost::shared_ptr<GenericUnaryFactor> sharedGenericUnaryFactor;
typedef std::vector<sharedGenericUnaryFactor> GenericUnaryGraph;
/** graph is a collection of factors */
typedef boost::shared_ptr<GenericUnaryFactor> sharedGenericUnaryFactor;
typedef std::vector<sharedGenericUnaryFactor> GenericUnaryGraph;
/***************************************************
* utility functions
***************************************************/
inline bool hasCommonCamera(const std::set<size_t>& cameras1, const std::set<size_t>& cameras2) {
if (cameras1.empty() || cameras2.empty())
throw std::invalid_argument("hasCommonCamera: the input camera set is empty!");
std::set<size_t>::const_iterator it1 = cameras1.begin();
std::set<size_t>::const_iterator it2 = cameras2.begin();
while (it1 != cameras1.end() && it2 != cameras2.end()) {
if (*it1 == *it2)
return true;
else if (*it1 < *it2)
it1++;
else
it2++;
}
return false;
}
/***************************************************
* utility functions
***************************************************/
inline bool hasCommonCamera(const std::set<size_t>& cameras1, const std::set<size_t>& cameras2) {
if (cameras1.empty() || cameras2.empty())
throw std::invalid_argument("hasCommonCamera: the input camera set is empty!");
std::set<size_t>::const_iterator it1 = cameras1.begin();
std::set<size_t>::const_iterator it2 = cameras2.begin();
while (it1 != cameras1.end() && it2 != cameras2.end()) {
if (*it1 == *it2)
return true;
else if (*it1 < *it2)
it1++;
else
it2++;
}
return false;
}
}} // namespace

404
gtsam_unstable/partition/NestedDissection-inl.h
View File

@ -16,236 +16,236 @@
namespace gtsam { namespace partition {
/* ************************************************************************* */
template <class NLG, class SubNLG, class GenericGraph>
NestedDissection<NLG, SubNLG, GenericGraph>::NestedDissection(
const NLG& fg, const Ordering& ordering, const int numNodeStopPartition, const int minNodesPerMap, const bool verbose) :
fg_(fg), ordering_(ordering){
GenericUnaryGraph unaryFactors;
GenericGraph gfg;
boost::tie(unaryFactors, gfg) = fg.createGenericGraph(ordering);
/* ************************************************************************* */
template <class NLG, class SubNLG, class GenericGraph>
NestedDissection<NLG, SubNLG, GenericGraph>::NestedDissection(
const NLG& fg, const Ordering& ordering, const int numNodeStopPartition, const int minNodesPerMap, const bool verbose) :
fg_(fg), ordering_(ordering){
GenericUnaryGraph unaryFactors;
GenericGraph gfg;
boost::tie(unaryFactors, gfg) = fg.createGenericGraph(ordering);
// build reverse mapping from integer to symbol
int numNodes = ordering.size();
int2symbol_.resize(numNodes);
Ordering::const_iterator it = ordering.begin(), itLast = ordering.end();
while(it != itLast)
int2symbol_[it->second] = (it++)->first;
// build reverse mapping from integer to symbol
int numNodes = ordering.size();
int2symbol_.resize(numNodes);
Ordering::const_iterator it = ordering.begin(), itLast = ordering.end();
while(it != itLast)
int2symbol_[it->second] = (it++)->first;
vector<size_t> keys;
keys.reserve(numNodes);
for(int i=0; i<ordering.size(); ++i)
keys.push_back(i);
vector<size_t> keys;
keys.reserve(numNodes);
for(int i=0; i<ordering.size(); ++i)
keys.push_back(i);
WorkSpace workspace(numNodes);
root_ = recursivePartition(gfg, unaryFactors, keys, vector<size_t>(), numNodeStopPartition, minNodesPerMap, boost::shared_ptr<SubNLG>(), workspace, verbose);
}
WorkSpace workspace(numNodes);
root_ = recursivePartition(gfg, unaryFactors, keys, vector<size_t>(), numNodeStopPartition, minNodesPerMap, boost::shared_ptr<SubNLG>(), workspace, verbose);
}
/* ************************************************************************* */
template <class NLG, class SubNLG, class GenericGraph>
NestedDissection<NLG, SubNLG, GenericGraph>::NestedDissection(
const NLG& fg, const Ordering& ordering, const boost::shared_ptr<Cuts>& cuts, const bool verbose) : fg_(fg), ordering_(ordering){
GenericUnaryGraph unaryFactors;
GenericGraph gfg;
boost::tie(unaryFactors, gfg) = fg.createGenericGraph(ordering);
/* ************************************************************************* */
template <class NLG, class SubNLG, class GenericGraph>
NestedDissection<NLG, SubNLG, GenericGraph>::NestedDissection(
const NLG& fg, const Ordering& ordering, const boost::shared_ptr<Cuts>& cuts, const bool verbose) : fg_(fg), ordering_(ordering){
GenericUnaryGraph unaryFactors;
GenericGraph gfg;
boost::tie(unaryFactors, gfg) = fg.createGenericGraph(ordering);
// build reverse mapping from integer to symbol
int numNodes = ordering.size();
int2symbol_.resize(numNodes);
Ordering::const_iterator it = ordering.begin(), itLast = ordering.end();
while(it != itLast)
int2symbol_[it->second] = (it++)->first;
// build reverse mapping from integer to symbol
int numNodes = ordering.size();
int2symbol_.resize(numNodes);
Ordering::const_iterator it = ordering.begin(), itLast = ordering.end();
while(it != itLast)
int2symbol_[it->second] = (it++)->first;
vector<size_t> keys;
keys.reserve(numNodes);
for(int i=0; i<ordering.size(); ++i)
keys.push_back(i);
vector<size_t> keys;
keys.reserve(numNodes);
for(int i=0; i<ordering.size(); ++i)
keys.push_back(i);
WorkSpace workspace(numNodes);
root_ = recursivePartition(gfg, unaryFactors, keys, vector<size_t>(), cuts, boost::shared_ptr<SubNLG>(), workspace, verbose);
}
WorkSpace workspace(numNodes);
root_ = recursivePartition(gfg, unaryFactors, keys, vector<size_t>(), cuts, boost::shared_ptr<SubNLG>(), workspace, verbose);
}
/* ************************************************************************* */
template <class NLG, class SubNLG, class GenericGraph>
boost::shared_ptr<SubNLG> NestedDissection<NLG, SubNLG, GenericGraph>::makeSubNLG(
const NLG& fg, const vector<size_t>& frontals, const vector<size_t>& sep, const boost::shared_ptr<SubNLG>& parent) const {
OrderedSymbols frontalKeys;
BOOST_FOREACH(const size_t index, frontals)
frontalKeys.push_back(int2symbol_[index]);
/* ************************************************************************* */
template <class NLG, class SubNLG, class GenericGraph>
boost::shared_ptr<SubNLG> NestedDissection<NLG, SubNLG, GenericGraph>::makeSubNLG(
const NLG& fg, const vector<size_t>& frontals, const vector<size_t>& sep, const boost::shared_ptr<SubNLG>& parent) const {
OrderedSymbols frontalKeys;
BOOST_FOREACH(const size_t index, frontals)
frontalKeys.push_back(int2symbol_[index]);
UnorderedSymbols sepKeys;
BOOST_FOREACH(const size_t index, sep)
sepKeys.insert(int2symbol_[index]);
UnorderedSymbols sepKeys;
BOOST_FOREACH(const size_t index, sep)
sepKeys.insert(int2symbol_[index]);
return boost::make_shared<SubNLG>(fg, frontalKeys, sepKeys, parent);
}
return boost::make_shared<SubNLG>(fg, frontalKeys, sepKeys, parent);
}
/* ************************************************************************* */
template <class NLG, class SubNLG, class GenericGraph>
void NestedDissection<NLG, SubNLG, GenericGraph>::processFactor(
const typename GenericGraph::value_type& factor, const std::vector<int>& partitionTable, // input
vector<GenericGraph>& frontalFactors, NLG& sepFactors, vector<set<size_t> >& childSeps, // output factor graphs
typename SubNLG::Weeklinks& weeklinks) const { // the links between child cliques
list<size_t> sep_; // the separator variables involved in the current factor
int partition1 = partitionTable[factor->key1.index];
int partition2 = partitionTable[factor->key2.index];
if (partition1 <= 0 && partition2 <= 0) { // is a factor in the current clique
sepFactors.push_back(fg_[factor->index]);
}
else if (partition1 > 0 && partition2 > 0 && partition1 != partition2) { // is a weeklink (factor between two child cliques)
weeklinks.push_back(fg_[factor->index]);
}
else if (partition1 > 0 && partition2 > 0 && partition1 == partition2) { // is a local factor in one of the child cliques
frontalFactors[partition1 - 1].push_back(factor);
}
else { // is a joint factor in the child clique (involving varaibles in the current clique)
if (partition1 > 0 && partition2 <= 0) {
frontalFactors[partition1 - 1].push_back(factor);
childSeps[partition1 - 1].insert(factor->key2.index);
} else if (partition1 <= 0 && partition2 > 0) {
frontalFactors[partition2 - 1].push_back(factor);
childSeps[partition2 - 1].insert(factor->key1.index);
} else
throw runtime_error("processFactor: unexpected entries in the partition table!");
}
}
/* ************************************************************************* */
template <class NLG, class SubNLG, class GenericGraph>
void NestedDissection<NLG, SubNLG, GenericGraph>::processFactor(
const typename GenericGraph::value_type& factor, const std::vector<int>& partitionTable, // input
vector<GenericGraph>& frontalFactors, NLG& sepFactors, vector<set<size_t> >& childSeps, // output factor graphs
typename SubNLG::Weeklinks& weeklinks) const { // the links between child cliques
list<size_t> sep_; // the separator variables involved in the current factor
int partition1 = partitionTable[factor->key1.index];
int partition2 = partitionTable[factor->key2.index];
if (partition1 <= 0 && partition2 <= 0) { // is a factor in the current clique
sepFactors.push_back(fg_[factor->index]);
}
else if (partition1 > 0 && partition2 > 0 && partition1 != partition2) { // is a weeklink (factor between two child cliques)
weeklinks.push_back(fg_[factor->index]);
}
else if (partition1 > 0 && partition2 > 0 && partition1 == partition2) { // is a local factor in one of the child cliques
frontalFactors[partition1 - 1].push_back(factor);
}
else { // is a joint factor in the child clique (involving varaibles in the current clique)
if (partition1 > 0 && partition2 <= 0) {
frontalFactors[partition1 - 1].push_back(factor);
childSeps[partition1 - 1].insert(factor->key2.index);
} else if (partition1 <= 0 && partition2 > 0) {
frontalFactors[partition2 - 1].push_back(factor);
childSeps[partition2 - 1].insert(factor->key1.index);
} else
throw runtime_error("processFactor: unexpected entries in the partition table!");
}
}
/* ************************************************************************* */
/**
* given a factor graph and its partition {nodeMap}, split the factors between the child cliques ({frontalFactors})
* and the current clique ({sepFactors}). Also split the variables between the child cliques ({childFrontals})
* and the current clique ({localFrontals}). Those separator variables involved in {frontalFactors} are put into
* the correspoding ordering in {childSeps}.
*/
// TODO: frontalFactors and localFrontals should be generated in findSeparator
template <class NLG, class SubNLG, class GenericGraph>
void NestedDissection<NLG, SubNLG, GenericGraph>::partitionFactorsAndVariables(
const GenericGraph& fg, const GenericUnaryGraph& unaryFactors, const std::vector<size_t>& keys, //input
const std::vector<int>& partitionTable, const int numSubmaps, // input
vector<GenericGraph>& frontalFactors, vector<GenericUnaryGraph>& frontalUnaryFactors, NLG& sepFactors, // output factor graphs
vector<vector<size_t> >& childFrontals, vector<vector<size_t> >& childSeps, vector<size_t>& localFrontals, // output sub-orderings
typename SubNLG::Weeklinks& weeklinks) const { // the links between child cliques
/* ************************************************************************* */
/**
* given a factor graph and its partition {nodeMap}, split the factors between the child cliques ({frontalFactors})
* and the current clique ({sepFactors}). Also split the variables between the child cliques ({childFrontals})
* and the current clique ({localFrontals}). Those separator variables involved in {frontalFactors} are put into
* the correspoding ordering in {childSeps}.
*/
// TODO: frontalFactors and localFrontals should be generated in findSeparator
template <class NLG, class SubNLG, class GenericGraph>
void NestedDissection<NLG, SubNLG, GenericGraph>::partitionFactorsAndVariables(
const GenericGraph& fg, const GenericUnaryGraph& unaryFactors, const std::vector<size_t>& keys, //input
const std::vector<int>& partitionTable, const int numSubmaps, // input
vector<GenericGraph>& frontalFactors, vector<GenericUnaryGraph>& frontalUnaryFactors, NLG& sepFactors, // output factor graphs
vector<vector<size_t> >& childFrontals, vector<vector<size_t> >& childSeps, vector<size_t>& localFrontals, // output sub-orderings
typename SubNLG::Weeklinks& weeklinks) const { // the links between child cliques
// make three lists of variables A, B, and C
int partition;
childFrontals.resize(numSubmaps);
BOOST_FOREACH(const size_t key, keys){
partition = partitionTable[key];
switch (partition) {
case -1: break; // the separator of the separator variables
case 0: localFrontals.push_back(key); break; // the separator variables
default: childFrontals[partition-1].push_back(key); // the frontal variables
}
}
// make three lists of variables A, B, and C
int partition;
childFrontals.resize(numSubmaps);
BOOST_FOREACH(const size_t key, keys){
partition = partitionTable[key];
switch (partition) {
case -1: break; // the separator of the separator variables
case 0: localFrontals.push_back(key); break; // the separator variables
default: childFrontals[partition-1].push_back(key); // the frontal variables
}
}
// group the factors to {frontalFactors} and {sepFactors},and find the joint variables
vector<set<size_t> > childSeps_;
childSeps_.resize(numSubmaps);
childSeps.reserve(numSubmaps);
frontalFactors.resize(numSubmaps);
frontalUnaryFactors.resize(numSubmaps);
BOOST_FOREACH(typename GenericGraph::value_type factor, fg)
processFactor(factor, partitionTable, frontalFactors, sepFactors, childSeps_, weeklinks);
BOOST_FOREACH(const set<size_t>& childSep, childSeps_)
childSeps.push_back(vector<size_t>(childSep.begin(), childSep.end()));
// group the factors to {frontalFactors} and {sepFactors},and find the joint variables
vector<set<size_t> > childSeps_;
childSeps_.resize(numSubmaps);
childSeps.reserve(numSubmaps);
frontalFactors.resize(numSubmaps);
frontalUnaryFactors.resize(numSubmaps);
BOOST_FOREACH(typename GenericGraph::value_type factor, fg)
processFactor(factor, partitionTable, frontalFactors, sepFactors, childSeps_, weeklinks);
BOOST_FOREACH(const set<size_t>& childSep, childSeps_)
childSeps.push_back(vector<size_t>(childSep.begin(), childSep.end()));
// add unary factor to the current cluster or pass it to one of the child clusters
BOOST_FOREACH(const sharedGenericUnaryFactor& unaryFactor_, unaryFactors) {
partition = partitionTable[unaryFactor_->key.index];
if (!partition) sepFactors.push_back(fg_[unaryFactor_->index]);
else frontalUnaryFactors[partition-1].push_back(unaryFactor_);
}
}
// add unary factor to the current cluster or pass it to one of the child clusters
BOOST_FOREACH(const sharedGenericUnaryFactor& unaryFactor_, unaryFactors) {
partition = partitionTable[unaryFactor_->key.index];
if (!partition) sepFactors.push_back(fg_[unaryFactor_->index]);
else frontalUnaryFactors[partition-1].push_back(unaryFactor_);
}
}
/* ************************************************************************* */
template <class NLG, class SubNLG, class GenericGraph>
NLG NestedDissection<NLG, SubNLG, GenericGraph>::collectOriginalFactors(
const GenericGraph& gfg, const GenericUnaryGraph& unaryFactors) const {
NLG sepFactors;
typename GenericGraph::const_iterator it = gfg.begin(), itLast = gfg.end();
while(it!=itLast) sepFactors.push_back(fg_[(*it++)->index]);
BOOST_FOREACH(const sharedGenericUnaryFactor& unaryFactor_, unaryFactors)
sepFactors.push_back(fg_[unaryFactor_->index]);
return sepFactors;
}
/* ************************************************************************* */
template <class NLG, class SubNLG, class GenericGraph>
NLG NestedDissection<NLG, SubNLG, GenericGraph>::collectOriginalFactors(
const GenericGraph& gfg, const GenericUnaryGraph& unaryFactors) const {
NLG sepFactors;
typename GenericGraph::const_iterator it = gfg.begin(), itLast = gfg.end();
while(it!=itLast) sepFactors.push_back(fg_[(*it++)->index]);
BOOST_FOREACH(const sharedGenericUnaryFactor& unaryFactor_, unaryFactors)
sepFactors.push_back(fg_[unaryFactor_->index]);
return sepFactors;
}
/* ************************************************************************* */
template <class NLG, class SubNLG, class GenericGraph>
boost::shared_ptr<SubNLG> NestedDissection<NLG, SubNLG, GenericGraph>::recursivePartition(
const GenericGraph& gfg, const GenericUnaryGraph& unaryFactors, const vector<size_t>& frontals, const vector<size_t>& sep,
const int numNodeStopPartition, const int minNodesPerMap, const boost::shared_ptr<SubNLG>& parent, WorkSpace& workspace, const bool verbose) const {
/* ************************************************************************* */
template <class NLG, class SubNLG, class GenericGraph>
boost::shared_ptr<SubNLG> NestedDissection<NLG, SubNLG, GenericGraph>::recursivePartition(
const GenericGraph& gfg, const GenericUnaryGraph& unaryFactors, const vector<size_t>& frontals, const vector<size_t>& sep,
const int numNodeStopPartition, const int minNodesPerMap, const boost::shared_ptr<SubNLG>& parent, WorkSpace& workspace, const bool verbose) const {
// if no split needed
NLG sepFactors; // factors that should remain in the current cluster
if (frontals.size() <= numNodeStopPartition || gfg.size() <= numNodeStopPartition) {
sepFactors = collectOriginalFactors(gfg, unaryFactors);
return makeSubNLG(sepFactors, frontals, sep, parent);
}
// if no split needed
NLG sepFactors; // factors that should remain in the current cluster
if (frontals.size() <= numNodeStopPartition || gfg.size() <= numNodeStopPartition) {
sepFactors = collectOriginalFactors(gfg, unaryFactors);
return makeSubNLG(sepFactors, frontals, sep, parent);
}
// find the nested dissection separator
int numSubmaps = findSeparator(gfg, frontals, minNodesPerMap, workspace, verbose, int2symbol_, NLG::reduceGraph(),
NLG::minNrConstraintsPerCamera(),NLG::minNrConstraintsPerLandmark());
partition::PartitionTable& partitionTable = workspace.partitionTable;
if (numSubmaps == 0) throw runtime_error("recursivePartition: get zero submap after ND!");
// find the nested dissection separator
int numSubmaps = findSeparator(gfg, frontals, minNodesPerMap, workspace, verbose, int2symbol_, NLG::reduceGraph(),
NLG::minNrConstraintsPerCamera(),NLG::minNrConstraintsPerLandmark());
partition::PartitionTable& partitionTable = workspace.partitionTable;
if (numSubmaps == 0) throw runtime_error("recursivePartition: get zero submap after ND!");
// split the factors between child cliques and the current clique
vector<GenericGraph> frontalFactors; vector<GenericUnaryGraph> frontalUnaryFactors; typename SubNLG::Weeklinks weeklinks;
vector<size_t> localFrontals; vector<vector<size_t> > childFrontals, childSeps;
partitionFactorsAndVariables(gfg, unaryFactors, frontals, partitionTable, numSubmaps,
frontalFactors, frontalUnaryFactors, sepFactors, childFrontals, childSeps, localFrontals, weeklinks);
// split the factors between child cliques and the current clique
vector<GenericGraph> frontalFactors; vector<GenericUnaryGraph> frontalUnaryFactors; typename SubNLG::Weeklinks weeklinks;
vector<size_t> localFrontals; vector<vector<size_t> > childFrontals, childSeps;
partitionFactorsAndVariables(gfg, unaryFactors, frontals, partitionTable, numSubmaps,
frontalFactors, frontalUnaryFactors, sepFactors, childFrontals, childSeps, localFrontals, weeklinks);
// make a new cluster
boost::shared_ptr<SubNLG> current = makeSubNLG(sepFactors, localFrontals, sep, parent);
current->setWeeklinks(weeklinks);
// make a new cluster
boost::shared_ptr<SubNLG> current = makeSubNLG(sepFactors, localFrontals, sep, parent);
current->setWeeklinks(weeklinks);
// check whether all the submaps are fully constrained
for (int i=0; i<numSubmaps; i++) {
checkSingularity(frontalFactors[i], childFrontals[i], workspace, NLG::minNrConstraintsPerCamera(),NLG::minNrConstraintsPerLandmark());
}
// check whether all the submaps are fully constrained
for (int i=0; i<numSubmaps; i++) {
checkSingularity(frontalFactors[i], childFrontals[i], workspace, NLG::minNrConstraintsPerCamera(),NLG::minNrConstraintsPerLandmark());
}
// create child clusters
for (int i=0; i<numSubmaps; i++) {
boost::shared_ptr<SubNLG> child = recursivePartition(frontalFactors[i], frontalUnaryFactors[i], childFrontals[i], childSeps[i],
numNodeStopPartition, minNodesPerMap, current, workspace, verbose);
current->addChild(child);
}
// create child clusters
for (int i=0; i<numSubmaps; i++) {
boost::shared_ptr<SubNLG> child = recursivePartition(frontalFactors[i], frontalUnaryFactors[i], childFrontals[i], childSeps[i],
numNodeStopPartition, minNodesPerMap, current, workspace, verbose);
current->addChild(child);
}
return current;
}
return current;
}
/* ************************************************************************* */
template <class NLG, class SubNLG, class GenericGraph>
boost::shared_ptr<SubNLG> NestedDissection<NLG, SubNLG, GenericGraph>::recursivePartition(
const GenericGraph& gfg, const GenericUnaryGraph& unaryFactors, const vector<size_t>& frontals, const vector<size_t>& sep,
const boost::shared_ptr<Cuts>& cuts, const boost::shared_ptr<SubNLG>& parent, WorkSpace& workspace, const bool verbose) const {
/* ************************************************************************* */
template <class NLG, class SubNLG, class GenericGraph>
boost::shared_ptr<SubNLG> NestedDissection<NLG, SubNLG, GenericGraph>::recursivePartition(
const GenericGraph& gfg, const GenericUnaryGraph& unaryFactors, const vector<size_t>& frontals, const vector<size_t>& sep,
const boost::shared_ptr<Cuts>& cuts, const boost::shared_ptr<SubNLG>& parent, WorkSpace& workspace, const bool verbose) const {
// if there is no need to cut any more
NLG sepFactors; // factors that should remain in the current cluster
if (!cuts.get()) {
sepFactors = collectOriginalFactors(gfg, unaryFactors);
return makeSubNLG(sepFactors, frontals, sep, parent);
}
// if there is no need to cut any more
NLG sepFactors; // factors that should remain in the current cluster
if (!cuts.get()) {
sepFactors = collectOriginalFactors(gfg, unaryFactors);
return makeSubNLG(sepFactors, frontals, sep, parent);
}
// retrieve the current partitioning info
int numSubmaps = 2;
partition::PartitionTable& partitionTable = cuts->partitionTable;
// retrieve the current partitioning info
int numSubmaps = 2;
partition::PartitionTable& partitionTable = cuts->partitionTable;
// split the factors between child cliques and the current clique
vector<GenericGraph> frontalFactors; vector<GenericUnaryGraph> frontalUnaryFactors; typename SubNLG::Weeklinks weeklinks;
vector<size_t> localFrontals; vector<vector<size_t> > childFrontals, childSeps;
partitionFactorsAndVariables(gfg, unaryFactors, frontals, partitionTable, numSubmaps,
frontalFactors, frontalUnaryFactors, sepFactors, childFrontals, childSeps, localFrontals, weeklinks);
// split the factors between child cliques and the current clique
vector<GenericGraph> frontalFactors; vector<GenericUnaryGraph> frontalUnaryFactors; typename SubNLG::Weeklinks weeklinks;
vector<size_t> localFrontals; vector<vector<size_t> > childFrontals, childSeps;
partitionFactorsAndVariables(gfg, unaryFactors, frontals, partitionTable, numSubmaps,
frontalFactors, frontalUnaryFactors, sepFactors, childFrontals, childSeps, localFrontals, weeklinks);
// make a new cluster
boost::shared_ptr<SubNLG> current = makeSubNLG(sepFactors, localFrontals, sep, parent);
current->setWeeklinks(weeklinks);
// make a new cluster
boost::shared_ptr<SubNLG> current = makeSubNLG(sepFactors, localFrontals, sep, parent);
current->setWeeklinks(weeklinks);
// create child clusters
for (int i=0; i<2; i++) {
boost::shared_ptr<SubNLG> child = recursivePartition(frontalFactors[i], frontalUnaryFactors[i], childFrontals[i], childSeps[i],
cuts->children.empty() ? boost::shared_ptr<Cuts>() : cuts->children[i], current, workspace, verbose);
current->addChild(child);
}
return current;
}
// create child clusters
for (int i=0; i<2; i++) {
boost::shared_ptr<SubNLG> child = recursivePartition(frontalFactors[i], frontalUnaryFactors[i], childFrontals[i], childSeps[i],
cuts->children.empty() ? boost::shared_ptr<Cuts>() : cuts->children[i], current, workspace, verbose);
current->addChild(child);
}
return current;
}
}} //namespace

80
gtsam_unstable/partition/NestedDissection.h
View File

@ -14,56 +14,56 @@
namespace gtsam { namespace partition {
/**
* Apply nested dissection algorithm to nonlinear factor graphs
*/
template <class NLG, class SubNLG, class GenericGraph>
class NestedDissection {
public:
typedef boost::shared_ptr<SubNLG> sharedSubNLG;
/**
* Apply nested dissection algorithm to nonlinear factor graphs
*/
template <class NLG, class SubNLG, class GenericGraph>
class NestedDissection {
public:
typedef boost::shared_ptr<SubNLG> sharedSubNLG;
private:
NLG fg_; // the original nonlinear factor graph
Ordering ordering_; // the variable ordering in the nonlinear factor graph
std::vector<Symbol> int2symbol_; // the mapping from integer key to symbol
sharedSubNLG root_; // the root of generated cluster tree
private:
NLG fg_; // the original nonlinear factor graph
Ordering ordering_; // the variable ordering in the nonlinear factor graph
std::vector<Symbol> int2symbol_; // the mapping from integer key to symbol
sharedSubNLG root_; // the root of generated cluster tree
public:
sharedSubNLG root() const { return root_; }
public:
sharedSubNLG root() const { return root_; }
public:
/* constructor with post-determined partitoning*/
NestedDissection(const NLG& fg, const Ordering& ordering, const int numNodeStopPartition, const int minNodesPerMap, const bool verbose = false);
public:
/* constructor with post-determined partitoning*/
NestedDissection(const NLG& fg, const Ordering& ordering, const int numNodeStopPartition, const int minNodesPerMap, const bool verbose = false);
/* constructor with pre-determined cuts*/
NestedDissection(const NLG& fg, const Ordering& ordering, const boost::shared_ptr<Cuts>& cuts, const bool verbose = false);
/* constructor with pre-determined cuts*/
NestedDissection(const NLG& fg, const Ordering& ordering, const boost::shared_ptr<Cuts>& cuts, const bool verbose = false);
private:
/* convert generic subgraph to nonlinear subgraph */
sharedSubNLG makeSubNLG(const NLG& fg, const std::vector<size_t>& frontals, const std::vector<size_t>& sep, const boost::shared_ptr<SubNLG>& parent) const;
private:
/* convert generic subgraph to nonlinear subgraph */
sharedSubNLG makeSubNLG(const NLG& fg, const std::vector<size_t>& frontals, const std::vector<size_t>& sep, const boost::shared_ptr<SubNLG>& parent) const;
void processFactor(const typename GenericGraph::value_type& factor, const std::vector<int>& partitionTable, // input
std::vector<GenericGraph>& frontalFactors, NLG& sepFactors, std::vector<std::set<size_t> >& childSeps, // output factor graphs
typename SubNLG::Weeklinks& weeklinks) const;
void processFactor(const typename GenericGraph::value_type& factor, const std::vector<int>& partitionTable, // input
std::vector<GenericGraph>& frontalFactors, NLG& sepFactors, std::vector<std::set<size_t> >& childSeps, // output factor graphs
typename SubNLG::Weeklinks& weeklinks) const;
/* recursively partition the generic graph */
void partitionFactorsAndVariables(
const GenericGraph& fg, const GenericUnaryGraph& unaryFactors,
const std::vector<size_t>& keys, const std::vector<int>& partitionTable, const int numSubmaps, // input
std::vector<GenericGraph>& frontalFactors, vector<GenericUnaryGraph>& frontalUnaryFactors, NLG& sepFactors, // output factor graphs
std::vector<std::vector<size_t> >& childFrontals, std::vector<std::vector<size_t> >& childSeps, std::vector<size_t>& localFrontals, // output sub-orderings
typename SubNLG::Weeklinks& weeklinks) const;
/* recursively partition the generic graph */
void partitionFactorsAndVariables(
const GenericGraph& fg, const GenericUnaryGraph& unaryFactors,
const std::vector<size_t>& keys, const std::vector<int>& partitionTable, const int numSubmaps, // input
std::vector<GenericGraph>& frontalFactors, vector<GenericUnaryGraph>& frontalUnaryFactors, NLG& sepFactors, // output factor graphs
std::vector<std::vector<size_t> >& childFrontals, std::vector<std::vector<size_t> >& childSeps, std::vector<size_t>& localFrontals, // output sub-orderings
typename SubNLG::Weeklinks& weeklinks) const;
NLG collectOriginalFactors(const GenericGraph& gfg, const GenericUnaryGraph& unaryFactors) const;
NLG collectOriginalFactors(const GenericGraph& gfg, const GenericUnaryGraph& unaryFactors) const;
/* recursively partition the generic graph */
sharedSubNLG recursivePartition(const GenericGraph& gfg, const GenericUnaryGraph& unaryFactors, const std::vector<size_t>& frontals, const std::vector<size_t>& sep,
const int numNodeStopPartition, const int minNodesPerMap, const boost::shared_ptr<SubNLG>& parent, WorkSpace& workspace, const bool verbose) const;
/* recursively partition the generic graph */
sharedSubNLG recursivePartition(const GenericGraph& gfg, const GenericUnaryGraph& unaryFactors, const std::vector<size_t>& frontals, const std::vector<size_t>& sep,
const int numNodeStopPartition, const int minNodesPerMap, const boost::shared_ptr<SubNLG>& parent, WorkSpace& workspace, const bool verbose) const;
/* recursively partition the generic graph */
sharedSubNLG recursivePartition(const GenericGraph& gfg, const GenericUnaryGraph& unaryFactors, const std::vector<size_t>& frontals, const std::vector<size_t>& sep,
const boost::shared_ptr<Cuts>& cuts, const boost::shared_ptr<SubNLG>& parent, WorkSpace& workspace, const bool verbose) const;
/* recursively partition the generic graph */
sharedSubNLG recursivePartition(const GenericGraph& gfg, const GenericUnaryGraph& unaryFactors, const std::vector<size_t>& frontals, const std::vector<size_t>& sep,
const boost::shared_ptr<Cuts>& cuts, const boost::shared_ptr<SubNLG>& parent, WorkSpace& workspace, const bool verbose) const;
};
};
}} //namespace

44
gtsam_unstable/partition/PartitionWorkSpace.h
View File

@ -13,32 +13,32 @@
namespace gtsam { namespace partition {
typedef std::vector<int> PartitionTable;
typedef std::vector<int> PartitionTable;
// the work space, preallocated memory
struct WorkSpace {
std::vector<int> dictionary; // a mapping from the integer key in the original graph to 0-based index in the subgraph, useful when handling a subset of keys and graphs
boost::shared_ptr<std::vector<size_t> > dsf; // a block memory pre-allocated for DSFVector
PartitionTable partitionTable; // a mapping from a key to the submap index, 0 means the separator, i means the ith submap
// the work space, preallocated memory
struct WorkSpace {
std::vector<int> dictionary; // a mapping from the integer key in the original graph to 0-based index in the subgraph, useful when handling a subset of keys and graphs
boost::shared_ptr<std::vector<size_t> > dsf; // a block memory pre-allocated for DSFVector
PartitionTable partitionTable; // a mapping from a key to the submap index, 0 means the separator, i means the ith submap
// constructor
WorkSpace(const size_t numNodes) : dictionary(numNodes,0),
dsf(new std::vector<size_t>(numNodes, 0)), partitionTable(numNodes, -1) { }
// constructor
WorkSpace(const size_t numNodes) : dictionary(numNodes,0),
dsf(new std::vector<size_t>(numNodes, 0)), partitionTable(numNodes, -1) { }
// set up dictionary: -1: no such key, none-zero: the corresponding 0-based index
inline void prepareDictionary(const std::vector<size_t>& keys) {
int index = 0;
std::fill(dictionary.begin(), dictionary.end(), -1);
std::vector<size_t>::const_iterator it=keys.begin(), itLast=keys.end();
while(it!=itLast) dictionary[*(it++)] = index++;
}
};
// set up dictionary: -1: no such key, none-zero: the corresponding 0-based index
inline void prepareDictionary(const std::vector<size_t>& keys) {
int index = 0;
std::fill(dictionary.begin(), dictionary.end(), -1);
std::vector<size_t>::const_iterator it=keys.begin(), itLast=keys.end();
while(it!=itLast) dictionary[*(it++)] = index++;
}
};
// manually defined cuts
struct Cuts {
PartitionTable partitionTable;
std::vector<boost::shared_ptr<Cuts> > children;
};
// manually defined cuts
struct Cuts {
PartitionTable partitionTable;
std::vector<boost::shared_ptr<Cuts> > children;
};
}} // namespace

70
gtsam_unstable/partition/tests/testFindSeparator.cpp
View File

@ -185,45 +185,45 @@ TEST ( Partition, findSeparator2 )
// x25 x26 x27 x28
TEST ( Partition, findSeparator3_with_reduced_camera )
{
GenericGraph3D graph;
for (int j=1; j<=8; j++)
graph.push_back(boost::make_shared<GenericFactor3D>(25, j));
for (int j=1; j<=16; j++)
graph.push_back(boost::make_shared<GenericFactor3D>(26, j));
for (int j=9; j<=24; j++)
graph.push_back(boost::make_shared<GenericFactor3D>(27, j));
for (int j=17; j<=24; j++)
graph.push_back(boost::make_shared<GenericFactor3D>(28, j));
GenericGraph3D graph;
for (int j=1; j<=8; j++)
graph.push_back(boost::make_shared<GenericFactor3D>(25, j));
for (int j=1; j<=16; j++)
graph.push_back(boost::make_shared<GenericFactor3D>(26, j));
for (int j=9; j<=24; j++)
graph.push_back(boost::make_shared<GenericFactor3D>(27, j));
for (int j=17; j<=24; j++)
graph.push_back(boost::make_shared<GenericFactor3D>(28, j));
std::vector<size_t> keys;
for(int i=1; i<=28; i++)
keys.push_back(i);
std::vector<size_t> keys;
for(int i=1; i<=28; i++)
keys.push_back(i);
vector<Symbol> int2symbol;
int2symbol.push_back(Symbol('x',0)); // dummy
for(int i=1; i<=24; i++)
int2symbol.push_back(Symbol('l',i));
int2symbol.push_back(Symbol('x',25));
int2symbol.push_back(Symbol('x',26));
int2symbol.push_back(Symbol('x',27));
int2symbol.push_back(Symbol('x',28));
vector<Symbol> int2symbol;
int2symbol.push_back(Symbol('x',0)); // dummy
for(int i=1; i<=24; i++)
int2symbol.push_back(Symbol('l',i));
int2symbol.push_back(Symbol('x',25));
int2symbol.push_back(Symbol('x',26));
int2symbol.push_back(Symbol('x',27));
int2symbol.push_back(Symbol('x',28));
WorkSpace workspace(29);
bool reduceGraph = true;
int numIsland = findSeparator(graph, keys, 3, workspace, false, int2symbol, reduceGraph, 0, 0);
LONGS_EQUAL(2, numIsland);
WorkSpace workspace(29);
bool reduceGraph = true;
int numIsland = findSeparator(graph, keys, 3, workspace, false, int2symbol, reduceGraph, 0, 0);
LONGS_EQUAL(2, numIsland);
partition::PartitionTable& partitionTable = workspace.partitionTable;
for (int j=1; j<=8; j++)
LONGS_EQUAL(1, partitionTable[j]);
for (int j=9; j<=16; j++)
LONGS_EQUAL(0, partitionTable[j]);
for (int j=17; j<=24; j++)
LONGS_EQUAL(2, partitionTable[j]);
LONGS_EQUAL(1, partitionTable[25]);
LONGS_EQUAL(1, partitionTable[26]);
LONGS_EQUAL(2, partitionTable[27]);
LONGS_EQUAL(2, partitionTable[28]);
partition::PartitionTable& partitionTable = workspace.partitionTable;
for (int j=1; j<=8; j++)
LONGS_EQUAL(1, partitionTable[j]);
for (int j=9; j<=16; j++)
LONGS_EQUAL(0, partitionTable[j]);
for (int j=17; j<=24; j++)
LONGS_EQUAL(2, partitionTable[j]);
LONGS_EQUAL(1, partitionTable[25]);
LONGS_EQUAL(1, partitionTable[26]);
LONGS_EQUAL(2, partitionTable[27]);
LONGS_EQUAL(2, partitionTable[28]);
}
/* ************************************************************************* */

270
gtsam_unstable/partition/tests/testGenericGraph.cpp
View File

@ -29,29 +29,29 @@ using namespace gtsam::partition;
*/
TEST ( GenerciGraph, findIslands )
{
GenericGraph2D graph;
graph.push_back(boost::make_shared<GenericFactor2D>(1, NODE_POSE_2D, 7, NODE_LANDMARK_2D));
graph.push_back(boost::make_shared<GenericFactor2D>(2, NODE_POSE_2D, 7, NODE_LANDMARK_2D));
graph.push_back(boost::make_shared<GenericFactor2D>(3, NODE_POSE_2D, 7, NODE_LANDMARK_2D));
graph.push_back(boost::make_shared<GenericFactor2D>(3, NODE_POSE_2D, 8, NODE_LANDMARK_2D));
graph.push_back(boost::make_shared<GenericFactor2D>(4, NODE_POSE_2D, 8, NODE_LANDMARK_2D));
graph.push_back(boost::make_shared<GenericFactor2D>(4, NODE_POSE_2D, 9, NODE_LANDMARK_2D));
graph.push_back(boost::make_shared<GenericFactor2D>(5, NODE_POSE_2D, 9, NODE_LANDMARK_2D));
graph.push_back(boost::make_shared<GenericFactor2D>(6, NODE_POSE_2D, 9, NODE_LANDMARK_2D));
GenericGraph2D graph;
graph.push_back(boost::make_shared<GenericFactor2D>(1, NODE_POSE_2D, 7, NODE_LANDMARK_2D));
graph.push_back(boost::make_shared<GenericFactor2D>(2, NODE_POSE_2D, 7, NODE_LANDMARK_2D));
graph.push_back(boost::make_shared<GenericFactor2D>(3, NODE_POSE_2D, 7, NODE_LANDMARK_2D));
graph.push_back(boost::make_shared<GenericFactor2D>(3, NODE_POSE_2D, 8, NODE_LANDMARK_2D));
graph.push_back(boost::make_shared<GenericFactor2D>(4, NODE_POSE_2D, 8, NODE_LANDMARK_2D));
graph.push_back(boost::make_shared<GenericFactor2D>(4, NODE_POSE_2D, 9, NODE_LANDMARK_2D));
graph.push_back(boost::make_shared<GenericFactor2D>(5, NODE_POSE_2D, 9, NODE_LANDMARK_2D));
graph.push_back(boost::make_shared<GenericFactor2D>(6, NODE_POSE_2D, 9, NODE_LANDMARK_2D));
graph.push_back(boost::make_shared<GenericFactor2D>(1, NODE_POSE_2D, 2, NODE_POSE_2D));
graph.push_back(boost::make_shared<GenericFactor2D>(2, NODE_POSE_2D, 3, NODE_POSE_2D));
graph.push_back(boost::make_shared<GenericFactor2D>(4, NODE_POSE_2D, 5, NODE_POSE_2D));
graph.push_back(boost::make_shared<GenericFactor2D>(5, NODE_POSE_2D, 6, NODE_POSE_2D));
std::vector<size_t> keys; keys += 1, 2, 3, 4, 5, 6, 7, 8, 9;
graph.push_back(boost::make_shared<GenericFactor2D>(1, NODE_POSE_2D, 2, NODE_POSE_2D));
graph.push_back(boost::make_shared<GenericFactor2D>(2, NODE_POSE_2D, 3, NODE_POSE_2D));
graph.push_back(boost::make_shared<GenericFactor2D>(4, NODE_POSE_2D, 5, NODE_POSE_2D));
graph.push_back(boost::make_shared<GenericFactor2D>(5, NODE_POSE_2D, 6, NODE_POSE_2D));
std::vector<size_t> keys; keys += 1, 2, 3, 4, 5, 6, 7, 8, 9;
WorkSpace workspace(10); // from 0 to 9
list<vector<size_t> > islands = findIslands(graph, keys, workspace, 7, 2);
LONGS_EQUAL(2, islands.size());
vector<size_t> island1; island1 += 1, 2, 3, 7, 8;
vector<size_t> island2; island2 += 4, 5, 6, 9;
CHECK(island1 == islands.front());
CHECK(island2 == islands.back());
WorkSpace workspace(10); // from 0 to 9
list<vector<size_t> > islands = findIslands(graph, keys, workspace, 7, 2);
LONGS_EQUAL(2, islands.size());
vector<size_t> island1; island1 += 1, 2, 3, 7, 8;
vector<size_t> island2; island2 += 4, 5, 6, 9;
CHECK(island1 == islands.front());
CHECK(island2 == islands.back());
}
/* ************************************************************************* */
@ -62,27 +62,27 @@ TEST ( GenerciGraph, findIslands )
*/
TEST( GenerciGraph, findIslands2 )
{
GenericGraph2D graph;
graph.push_back(boost::make_shared<GenericFactor2D>(1, NODE_POSE_2D, 7, NODE_LANDMARK_2D));
graph.push_back(boost::make_shared<GenericFactor2D>(2, NODE_POSE_2D, 7, NODE_LANDMARK_2D));
graph.push_back(boost::make_shared<GenericFactor2D>(3, NODE_POSE_2D, 7, NODE_LANDMARK_2D));
graph.push_back(boost::make_shared<GenericFactor2D>(3, NODE_POSE_2D, 8, NODE_LANDMARK_2D));
graph.push_back(boost::make_shared<GenericFactor2D>(4, NODE_POSE_2D, 7, NODE_LANDMARK_2D));
graph.push_back(boost::make_shared<GenericFactor2D>(4, NODE_POSE_2D, 8, NODE_LANDMARK_2D));
graph.push_back(boost::make_shared<GenericFactor2D>(5, NODE_POSE_2D, 8, NODE_LANDMARK_2D));
graph.push_back(boost::make_shared<GenericFactor2D>(6, NODE_POSE_2D, 8, NODE_LANDMARK_2D));
GenericGraph2D graph;
graph.push_back(boost::make_shared<GenericFactor2D>(1, NODE_POSE_2D, 7, NODE_LANDMARK_2D));
graph.push_back(boost::make_shared<GenericFactor2D>(2, NODE_POSE_2D, 7, NODE_LANDMARK_2D));
graph.push_back(boost::make_shared<GenericFactor2D>(3, NODE_POSE_2D, 7, NODE_LANDMARK_2D));
graph.push_back(boost::make_shared<GenericFactor2D>(3, NODE_POSE_2D, 8, NODE_LANDMARK_2D));
graph.push_back(boost::make_shared<GenericFactor2D>(4, NODE_POSE_2D, 7, NODE_LANDMARK_2D));
graph.push_back(boost::make_shared<GenericFactor2D>(4, NODE_POSE_2D, 8, NODE_LANDMARK_2D));
graph.push_back(boost::make_shared<GenericFactor2D>(5, NODE_POSE_2D, 8, NODE_LANDMARK_2D));
graph.push_back(boost::make_shared<GenericFactor2D>(6, NODE_POSE_2D, 8, NODE_LANDMARK_2D));
graph.push_back(boost::make_shared<GenericFactor2D>(1, NODE_POSE_2D, 2, NODE_POSE_2D));
graph.push_back(boost::make_shared<GenericFactor2D>(2, NODE_POSE_2D, 3, NODE_POSE_2D));
graph.push_back(boost::make_shared<GenericFactor2D>(4, NODE_POSE_2D, 5, NODE_POSE_2D));
graph.push_back(boost::make_shared<GenericFactor2D>(5, NODE_POSE_2D, 6, NODE_POSE_2D));
std::vector<size_t> keys; keys += 1, 2, 3, 4, 5, 6, 7, 8;
graph.push_back(boost::make_shared<GenericFactor2D>(1, NODE_POSE_2D, 2, NODE_POSE_2D));
graph.push_back(boost::make_shared<GenericFactor2D>(2, NODE_POSE_2D, 3, NODE_POSE_2D));
graph.push_back(boost::make_shared<GenericFactor2D>(4, NODE_POSE_2D, 5, NODE_POSE_2D));
graph.push_back(boost::make_shared<GenericFactor2D>(5, NODE_POSE_2D, 6, NODE_POSE_2D));
std::vector<size_t> keys; keys += 1, 2, 3, 4, 5, 6, 7, 8;
WorkSpace workspace(15); // from 0 to 8, but testing over-allocation here
list<vector<size_t> > islands = findIslands(graph, keys, workspace, 7, 2);
LONGS_EQUAL(1, islands.size());
vector<size_t> island1; island1 += 1, 2, 3, 4, 5, 6, 7, 8;
CHECK(island1 == islands.front());
WorkSpace workspace(15); // from 0 to 8, but testing over-allocation here
list<vector<size_t> > islands = findIslands(graph, keys, workspace, 7, 2);
LONGS_EQUAL(1, islands.size());
vector<size_t> island1; island1 += 1, 2, 3, 4, 5, 6, 7, 8;
CHECK(island1 == islands.front());
}
/* ************************************************************************* */
@ -92,21 +92,21 @@ TEST( GenerciGraph, findIslands2 )
*/
TEST ( GenerciGraph, findIslands3 )
{
GenericGraph2D graph;
graph.push_back(boost::make_shared<GenericFactor2D>(1, NODE_POSE_2D, 5, NODE_LANDMARK_2D));
graph.push_back(boost::make_shared<GenericFactor2D>(4, NODE_POSE_2D, 6, NODE_LANDMARK_2D));
GenericGraph2D graph;
graph.push_back(boost::make_shared<GenericFactor2D>(1, NODE_POSE_2D, 5, NODE_LANDMARK_2D));
graph.push_back(boost::make_shared<GenericFactor2D>(4, NODE_POSE_2D, 6, NODE_LANDMARK_2D));
graph.push_back(boost::make_shared<GenericFactor2D>(2, NODE_POSE_2D, 3, NODE_POSE_2D));
graph.push_back(boost::make_shared<GenericFactor2D>(3, NODE_POSE_2D, 4, NODE_POSE_2D));
std::vector<size_t> keys; keys += 1, 2, 3, 4, 5, 6;
graph.push_back(boost::make_shared<GenericFactor2D>(2, NODE_POSE_2D, 3, NODE_POSE_2D));
graph.push_back(boost::make_shared<GenericFactor2D>(3, NODE_POSE_2D, 4, NODE_POSE_2D));
std::vector<size_t> keys; keys += 1, 2, 3, 4, 5, 6;
WorkSpace workspace(7); // from 0 to 9
list<vector<size_t> > islands = findIslands(graph, keys, workspace, 7, 2);
LONGS_EQUAL(2, islands.size());
vector<size_t> island1; island1 += 1, 5;
vector<size_t> island2; island2 += 2, 3, 4, 6;
CHECK(island1 == islands.front());
CHECK(island2 == islands.back());
WorkSpace workspace(7); // from 0 to 9
list<vector<size_t> > islands = findIslands(graph, keys, workspace, 7, 2);
LONGS_EQUAL(2, islands.size());
vector<size_t> island1; island1 += 1, 5;
vector<size_t> island2; island2 += 2, 3, 4, 6;
CHECK(island1 == islands.front());
CHECK(island2 == islands.back());
}
/* ************************************************************************* */
@ -115,18 +115,18 @@ TEST ( GenerciGraph, findIslands3 )
*/
TEST ( GenerciGraph, findIslands4 )
{
GenericGraph2D graph;
graph.push_back(boost::make_shared<GenericFactor2D>(3, NODE_POSE_2D, 4, NODE_LANDMARK_2D));
graph.push_back(boost::make_shared<GenericFactor2D>(7, NODE_POSE_2D, 7, NODE_LANDMARK_2D));
std::vector<size_t> keys; keys += 3, 4, 7;
GenericGraph2D graph;
graph.push_back(boost::make_shared<GenericFactor2D>(3, NODE_POSE_2D, 4, NODE_LANDMARK_2D));
graph.push_back(boost::make_shared<GenericFactor2D>(7, NODE_POSE_2D, 7, NODE_LANDMARK_2D));
std::vector<size_t> keys; keys += 3, 4, 7;
WorkSpace workspace(8); // from 0 to 7
list<vector<size_t> > islands = findIslands(graph, keys, workspace, 7, 2);
LONGS_EQUAL(2, islands.size());
vector<size_t> island1; island1 += 3, 4;
vector<size_t> island2; island2 += 7;
CHECK(island1 == islands.front());
CHECK(island2 == islands.back());
WorkSpace workspace(8); // from 0 to 7
list<vector<size_t> > islands = findIslands(graph, keys, workspace, 7, 2);
LONGS_EQUAL(2, islands.size());
vector<size_t> island1; island1 += 3, 4;
vector<size_t> island2; island2 += 7;
CHECK(island1 == islands.front());
CHECK(island2 == islands.back());
}
/* ************************************************************************* */
@ -137,24 +137,24 @@ TEST ( GenerciGraph, findIslands4 )
*/
TEST ( GenerciGraph, findIslands5 )
{
GenericGraph2D graph;
graph.push_back(boost::make_shared<GenericFactor2D>(1, NODE_POSE_2D, 5, NODE_LANDMARK_2D));
graph.push_back(boost::make_shared<GenericFactor2D>(2, NODE_POSE_2D, 5, NODE_LANDMARK_2D));
graph.push_back(boost::make_shared<GenericFactor2D>(3, NODE_POSE_2D, 5, NODE_LANDMARK_2D));
graph.push_back(boost::make_shared<GenericFactor2D>(4, NODE_POSE_2D, 5, NODE_LANDMARK_2D));
GenericGraph2D graph;
graph.push_back(boost::make_shared<GenericFactor2D>(1, NODE_POSE_2D, 5, NODE_LANDMARK_2D));
graph.push_back(boost::make_shared<GenericFactor2D>(2, NODE_POSE_2D, 5, NODE_LANDMARK_2D));
graph.push_back(boost::make_shared<GenericFactor2D>(3, NODE_POSE_2D, 5, NODE_LANDMARK_2D));
graph.push_back(boost::make_shared<GenericFactor2D>(4, NODE_POSE_2D, 5, NODE_LANDMARK_2D));
graph.push_back(boost::make_shared<GenericFactor2D>(1, NODE_POSE_2D, 3, NODE_POSE_2D));
graph.push_back(boost::make_shared<GenericFactor2D>(2, NODE_POSE_2D, 4, NODE_POSE_2D));
graph.push_back(boost::make_shared<GenericFactor2D>(1, NODE_POSE_2D, 3, NODE_POSE_2D));
graph.push_back(boost::make_shared<GenericFactor2D>(2, NODE_POSE_2D, 4, NODE_POSE_2D));
std::vector<size_t> keys; keys += 1, 2, 3, 4, 5;
std::vector<size_t> keys; keys += 1, 2, 3, 4, 5;
WorkSpace workspace(6); // from 0 to 5
list<vector<size_t> > islands = findIslands(graph, keys, workspace, 7, 2);
LONGS_EQUAL(2, islands.size());
vector<size_t> island1; island1 += 1, 3, 5;
vector<size_t> island2; island2 += 2, 4;
CHECK(island1 == islands.front());
CHECK(island2 == islands.back());
WorkSpace workspace(6); // from 0 to 5
list<vector<size_t> > islands = findIslands(graph, keys, workspace, 7, 2);
LONGS_EQUAL(2, islands.size());
vector<size_t> island1; island1 += 1, 3, 5;
vector<size_t> island2; island2 += 2, 4;
CHECK(island1 == islands.front());
CHECK(island2 == islands.back());
}
/* ************************************************************************* */
@ -165,31 +165,31 @@ TEST ( GenerciGraph, findIslands5 )
*/
TEST ( GenerciGraph, reduceGenericGraph )
{
GenericGraph3D graph;
graph.push_back(boost::make_shared<GenericFactor3D>(1, 3));
graph.push_back(boost::make_shared<GenericFactor3D>(1, 4));
graph.push_back(boost::make_shared<GenericFactor3D>(1, 5));
graph.push_back(boost::make_shared<GenericFactor3D>(2, 5));
graph.push_back(boost::make_shared<GenericFactor3D>(2, 6));
GenericGraph3D graph;
graph.push_back(boost::make_shared<GenericFactor3D>(1, 3));
graph.push_back(boost::make_shared<GenericFactor3D>(1, 4));
graph.push_back(boost::make_shared<GenericFactor3D>(1, 5));
graph.push_back(boost::make_shared<GenericFactor3D>(2, 5));
graph.push_back(boost::make_shared<GenericFactor3D>(2, 6));
std::vector<size_t> cameraKeys, landmarkKeys;
cameraKeys.push_back(1);
cameraKeys.push_back(2);
landmarkKeys.push_back(3);
landmarkKeys.push_back(4);
landmarkKeys.push_back(5);
landmarkKeys.push_back(6);
std::vector<size_t> cameraKeys, landmarkKeys;
cameraKeys.push_back(1);
cameraKeys.push_back(2);
landmarkKeys.push_back(3);
landmarkKeys.push_back(4);
landmarkKeys.push_back(5);
landmarkKeys.push_back(6);
std::vector<int> dictionary;
dictionary.resize(7, -1); // from 0 to 6
dictionary[1] = 0;
dictionary[2] = 1;
std::vector<int> dictionary;
dictionary.resize(7, -1); // from 0 to 6
dictionary[1] = 0;
dictionary[2] = 1;
GenericGraph3D reduced;
std::map<size_t, vector<size_t> > cameraToLandmarks;
reduceGenericGraph(graph, cameraKeys, landmarkKeys, dictionary, reduced);
LONGS_EQUAL(1, reduced.size());
LONGS_EQUAL(1, reduced[0]->key1.index); LONGS_EQUAL(2, reduced[0]->key2.index);
GenericGraph3D reduced;
std::map<size_t, vector<size_t> > cameraToLandmarks;
reduceGenericGraph(graph, cameraKeys, landmarkKeys, dictionary, reduced);
LONGS_EQUAL(1, reduced.size());
LONGS_EQUAL(1, reduced[0]->key1.index); LONGS_EQUAL(2, reduced[0]->key2.index);
}
/* ************************************************************************* */
@ -200,53 +200,53 @@ TEST ( GenerciGraph, reduceGenericGraph )
*/
TEST ( GenericGraph, reduceGenericGraph2 )
{
GenericGraph3D graph;
graph.push_back(boost::make_shared<GenericFactor3D>(1, 3, 0, NODE_POSE_3D, NODE_LANDMARK_3D));
graph.push_back(boost::make_shared<GenericFactor3D>(1, 4, 1, NODE_POSE_3D, NODE_LANDMARK_3D));
graph.push_back(boost::make_shared<GenericFactor3D>(1, 5, 2, NODE_POSE_3D, NODE_LANDMARK_3D));
graph.push_back(boost::make_shared<GenericFactor3D>(2, 5, 3, NODE_POSE_3D, NODE_LANDMARK_3D));
graph.push_back(boost::make_shared<GenericFactor3D>(2, 6, 4, NODE_POSE_3D, NODE_LANDMARK_3D));
graph.push_back(boost::make_shared<GenericFactor3D>(2, 7, 5, NODE_POSE_3D, NODE_POSE_3D));
GenericGraph3D graph;
graph.push_back(boost::make_shared<GenericFactor3D>(1, 3, 0, NODE_POSE_3D, NODE_LANDMARK_3D));
graph.push_back(boost::make_shared<GenericFactor3D>(1, 4, 1, NODE_POSE_3D, NODE_LANDMARK_3D));
graph.push_back(boost::make_shared<GenericFactor3D>(1, 5, 2, NODE_POSE_3D, NODE_LANDMARK_3D));
graph.push_back(boost::make_shared<GenericFactor3D>(2, 5, 3, NODE_POSE_3D, NODE_LANDMARK_3D));
graph.push_back(boost::make_shared<GenericFactor3D>(2, 6, 4, NODE_POSE_3D, NODE_LANDMARK_3D));
graph.push_back(boost::make_shared<GenericFactor3D>(2, 7, 5, NODE_POSE_3D, NODE_POSE_3D));
std::vector<size_t> cameraKeys, landmarkKeys;
cameraKeys.push_back(1);
cameraKeys.push_back(2);
cameraKeys.push_back(7);
landmarkKeys.push_back(3);
landmarkKeys.push_back(4);
landmarkKeys.push_back(5);
landmarkKeys.push_back(6);
std::vector<size_t> cameraKeys, landmarkKeys;
cameraKeys.push_back(1);
cameraKeys.push_back(2);
cameraKeys.push_back(7);
landmarkKeys.push_back(3);
landmarkKeys.push_back(4);
landmarkKeys.push_back(5);
landmarkKeys.push_back(6);
std::vector<int> dictionary;
dictionary.resize(8, -1); // from 0 to 7
dictionary[1] = 0;
dictionary[2] = 1;
dictionary[7] = 6;
std::vector<int> dictionary;
dictionary.resize(8, -1); // from 0 to 7
dictionary[1] = 0;
dictionary[2] = 1;
dictionary[7] = 6;
GenericGraph3D reduced;
std::map<size_t, vector<size_t> > cameraToLandmarks;
reduceGenericGraph(graph, cameraKeys, landmarkKeys, dictionary, reduced);
LONGS_EQUAL(2, reduced.size());
LONGS_EQUAL(1, reduced[0]->key1.index); LONGS_EQUAL(2, reduced[0]->key2.index);
LONGS_EQUAL(2, reduced[1]->key1.index); LONGS_EQUAL(7, reduced[1]->key2.index);
GenericGraph3D reduced;
std::map<size_t, vector<size_t> > cameraToLandmarks;
reduceGenericGraph(graph, cameraKeys, landmarkKeys, dictionary, reduced);
LONGS_EQUAL(2, reduced.size());
LONGS_EQUAL(1, reduced[0]->key1.index); LONGS_EQUAL(2, reduced[0]->key2.index);
LONGS_EQUAL(2, reduced[1]->key1.index); LONGS_EQUAL(7, reduced[1]->key2.index);
}
/* ************************************************************************* */
TEST ( GenerciGraph, hasCommonCamera )
{
std::set<size_t> cameras1; cameras1 += 1, 2, 3, 4, 5;
std::set<size_t> cameras2; cameras2 += 8, 7, 6, 5;
bool actual = hasCommonCamera(cameras1, cameras2);
CHECK(actual);
std::set<size_t> cameras1; cameras1 += 1, 2, 3, 4, 5;
std::set<size_t> cameras2; cameras2 += 8, 7, 6, 5;
bool actual = hasCommonCamera(cameras1, cameras2);
CHECK(actual);
}
/* ************************************************************************* */
TEST ( GenerciGraph, hasCommonCamera2 )
{
std::set<size_t> cameras1; cameras1 += 1, 3, 5, 7;
std::set<size_t> cameras2; cameras2 += 2, 4, 6, 8, 10;
bool actual = hasCommonCamera(cameras1, cameras2);
CHECK(!actual);
std::set<size_t> cameras1; cameras1 += 1, 3, 5, 7;
std::set<size_t> cameras2; cameras2 += 2, 4, 6, 8, 10;
bool actual = hasCommonCamera(cameras1, cameras2);
CHECK(!actual);
}
/* ************************************************************************* */

434
gtsam_unstable/partition/tests/testNestedDissection.cpp
View File

@ -32,22 +32,22 @@ using namespace gtsam::partition;
// l1
TEST ( NestedDissection, oneIsland )
{
using namespace submapPlanarSLAM;
typedef TSAM2D::SubNLG SubNLG;
Graph fg;
fg.addOdometry(1, 2, Pose2(), odoNoise);
fg.addBearingRange(1, 1, Rot2(), 0., bearingRangeNoise);
fg.addBearingRange(2, 1, Rot2(), 0., bearingRangeNoise);
fg.addPoseConstraint(1, Pose2());
using namespace submapPlanarSLAM;
typedef TSAM2D::SubNLG SubNLG;
Graph fg;
fg.addOdometry(1, 2, Pose2(), odoNoise);
fg.addBearingRange(1, 1, Rot2(), 0., bearingRangeNoise);
fg.addBearingRange(2, 1, Rot2(), 0., bearingRangeNoise);
fg.addPoseConstraint(1, Pose2());
Ordering ordering; ordering += x1, x2, l1;
Ordering ordering; ordering += x1, x2, l1;
int numNodeStopPartition = 1e3;
int minNodesPerMap = 1e3;
NestedDissection<Graph, SubNLG, GenericGraph2D> nd(fg, ordering, numNodeStopPartition, minNodesPerMap);
LONGS_EQUAL(4, nd.root()->size());
LONGS_EQUAL(3, nd.root()->frontal().size());
LONGS_EQUAL(0, nd.root()->children().size());
int numNodeStopPartition = 1e3;
int minNodesPerMap = 1e3;
NestedDissection<Graph, SubNLG, GenericGraph2D> nd(fg, ordering, numNodeStopPartition, minNodesPerMap);
LONGS_EQUAL(4, nd.root()->size());
LONGS_EQUAL(3, nd.root()->frontal().size());
LONGS_EQUAL(0, nd.root()->children().size());
}
/* ************************************************************************* */
@ -56,35 +56,35 @@ TEST ( NestedDissection, oneIsland )
// x2/ \x5
TEST ( NestedDissection, TwoIslands )
{
using namespace submapPlanarSLAM;
typedef TSAM2D::SubNLG SubNLG;
Graph fg;
fg.addOdometry(1, 2, Pose2(), odoNoise);
fg.addOdometry(1, 3, Pose2(), odoNoise);
fg.addOdometry(2, 3, Pose2(), odoNoise);
fg.addOdometry(3, 4, Pose2(), odoNoise);
fg.addOdometry(4, 5, Pose2(), odoNoise);
fg.addOdometry(3, 5, Pose2(), odoNoise);
fg.addPoseConstraint(1, Pose2());
fg.addPoseConstraint(4, Pose2());
Ordering ordering; ordering += x1, x2, x3, x4, x5;
using namespace submapPlanarSLAM;
typedef TSAM2D::SubNLG SubNLG;
Graph fg;
fg.addOdometry(1, 2, Pose2(), odoNoise);
fg.addOdometry(1, 3, Pose2(), odoNoise);
fg.addOdometry(2, 3, Pose2(), odoNoise);
fg.addOdometry(3, 4, Pose2(), odoNoise);
fg.addOdometry(4, 5, Pose2(), odoNoise);
fg.addOdometry(3, 5, Pose2(), odoNoise);
fg.addPoseConstraint(1, Pose2());
fg.addPoseConstraint(4, Pose2());
Ordering ordering; ordering += x1, x2, x3, x4, x5;
int numNodeStopPartition = 2;
int minNodesPerMap = 1;
NestedDissection<Graph, SubNLG, GenericGraph2D> nd(fg, ordering, numNodeStopPartition, minNodesPerMap);
// root submap
LONGS_EQUAL(0, nd.root()->size());
LONGS_EQUAL(1, nd.root()->frontal().size());
LONGS_EQUAL(0, nd.root()->separator().size());
LONGS_EQUAL(2, nd.root()->children().size()); // 2 leaf submaps
int numNodeStopPartition = 2;
int minNodesPerMap = 1;
NestedDissection<Graph, SubNLG, GenericGraph2D> nd(fg, ordering, numNodeStopPartition, minNodesPerMap);
// root submap
LONGS_EQUAL(0, nd.root()->size());
LONGS_EQUAL(1, nd.root()->frontal().size());
LONGS_EQUAL(0, nd.root()->separator().size());
LONGS_EQUAL(2, nd.root()->children().size()); // 2 leaf submaps
// the 1st submap
LONGS_EQUAL(2, nd.root()->children()[0]->frontal().size());
LONGS_EQUAL(4, nd.root()->children()[0]->size());
// the 1st submap
LONGS_EQUAL(2, nd.root()->children()[0]->frontal().size());
LONGS_EQUAL(4, nd.root()->children()[0]->size());
// the 2nd submap
LONGS_EQUAL(2, nd.root()->children()[1]->frontal().size());
LONGS_EQUAL(4, nd.root()->children()[1]->size());
// the 2nd submap
LONGS_EQUAL(2, nd.root()->children()[1]->frontal().size());
LONGS_EQUAL(4, nd.root()->children()[1]->size());
}
/* ************************************************************************* */
@ -93,40 +93,40 @@ TEST ( NestedDissection, TwoIslands )
// x2/ \x5
TEST ( NestedDissection, FourIslands )
{
using namespace submapPlanarSLAM;
typedef TSAM2D::SubNLG SubNLG;
Graph fg;
fg.addOdometry(1, 3, Pose2(), odoNoise);
fg.addOdometry(2, 3, Pose2(), odoNoise);
fg.addOdometry(3, 4, Pose2(), odoNoise);
fg.addOdometry(3, 5, Pose2(), odoNoise);
fg.addPoseConstraint(1, Pose2());
fg.addPoseConstraint(4, Pose2());
Ordering ordering; ordering += x1, x2, x3, x4, x5;
using namespace submapPlanarSLAM;
typedef TSAM2D::SubNLG SubNLG;
Graph fg;
fg.addOdometry(1, 3, Pose2(), odoNoise);
fg.addOdometry(2, 3, Pose2(), odoNoise);
fg.addOdometry(3, 4, Pose2(), odoNoise);
fg.addOdometry(3, 5, Pose2(), odoNoise);
fg.addPoseConstraint(1, Pose2());
fg.addPoseConstraint(4, Pose2());
Ordering ordering; ordering += x1, x2, x3, x4, x5;
int numNodeStopPartition = 2;
int minNodesPerMap = 1;
NestedDissection<Graph, SubNLG, GenericGraph2D> nd(fg, ordering, numNodeStopPartition, minNodesPerMap);
LONGS_EQUAL(0, nd.root()->size());
LONGS_EQUAL(1, nd.root()->frontal().size());
LONGS_EQUAL(0, nd.root()->separator().size());
LONGS_EQUAL(4, nd.root()->children().size()); // 4 leaf submaps
int numNodeStopPartition = 2;
int minNodesPerMap = 1;
NestedDissection<Graph, SubNLG, GenericGraph2D> nd(fg, ordering, numNodeStopPartition, minNodesPerMap);
LONGS_EQUAL(0, nd.root()->size());
LONGS_EQUAL(1, nd.root()->frontal().size());
LONGS_EQUAL(0, nd.root()->separator().size());
LONGS_EQUAL(4, nd.root()->children().size()); // 4 leaf submaps
// the 1st submap
LONGS_EQUAL(1, nd.root()->children()[0]->frontal().size());
LONGS_EQUAL(2, nd.root()->children()[0]->size());
// the 1st submap
LONGS_EQUAL(1, nd.root()->children()[0]->frontal().size());
LONGS_EQUAL(2, nd.root()->children()[0]->size());
// the 2nd submap
LONGS_EQUAL(1, nd.root()->children()[1]->frontal().size());
LONGS_EQUAL(2, nd.root()->children()[1]->size());
// the 2nd submap
LONGS_EQUAL(1, nd.root()->children()[1]->frontal().size());
LONGS_EQUAL(2, nd.root()->children()[1]->size());
// the 3rd submap
LONGS_EQUAL(1, nd.root()->children()[2]->frontal().size());
LONGS_EQUAL(1, nd.root()->children()[2]->size());
// the 3rd submap
LONGS_EQUAL(1, nd.root()->children()[2]->frontal().size());
LONGS_EQUAL(1, nd.root()->children()[2]->size());
// the 4th submap
LONGS_EQUAL(1, nd.root()->children()[3]->frontal().size());
LONGS_EQUAL(1, nd.root()->children()[3]->size());
// the 4th submap
LONGS_EQUAL(1, nd.root()->children()[3]->frontal().size());
LONGS_EQUAL(1, nd.root()->children()[3]->size());
}
/* ************************************************************************* */
@ -137,41 +137,41 @@ TEST ( NestedDissection, FourIslands )
// x5
TEST ( NestedDissection, weekLinks )
{
using namespace submapPlanarSLAM;
typedef TSAM2D::SubNLG SubNLG;
Graph fg;
fg.addOdometry(1, 2, Pose2(), odoNoise);
fg.addOdometry(2, 3, Pose2(), odoNoise);
fg.addOdometry(2, 4, Pose2(), odoNoise);
fg.addOdometry(3, 4, Pose2(), odoNoise);
fg.addBearingRange(1, 6, Rot2(), 0., bearingRangeNoise);
fg.addBearingRange(2, 6, Rot2(), 0., bearingRangeNoise);
fg.addBearingRange(5, 6, Rot2(), 0., bearingRangeNoise);
fg.addPoseConstraint(1, Pose2());
fg.addPoseConstraint(4, Pose2());
fg.addPoseConstraint(5, Pose2());
Ordering ordering; ordering += x1, x2, x3, x4, x5, l6;
using namespace submapPlanarSLAM;
typedef TSAM2D::SubNLG SubNLG;
Graph fg;
fg.addOdometry(1, 2, Pose2(), odoNoise);
fg.addOdometry(2, 3, Pose2(), odoNoise);
fg.addOdometry(2, 4, Pose2(), odoNoise);
fg.addOdometry(3, 4, Pose2(), odoNoise);
fg.addBearingRange(1, 6, Rot2(), 0., bearingRangeNoise);
fg.addBearingRange(2, 6, Rot2(), 0., bearingRangeNoise);
fg.addBearingRange(5, 6, Rot2(), 0., bearingRangeNoise);
fg.addPoseConstraint(1, Pose2());
fg.addPoseConstraint(4, Pose2());
fg.addPoseConstraint(5, Pose2());
Ordering ordering; ordering += x1, x2, x3, x4, x5, l6;
int numNodeStopPartition = 2;
int minNodesPerMap = 1;
NestedDissection<Graph, SubNLG, GenericGraph2D> nd(fg, ordering, numNodeStopPartition, minNodesPerMap);
LONGS_EQUAL(0, nd.root()->size()); // one weeklink
LONGS_EQUAL(1, nd.root()->frontal().size());
LONGS_EQUAL(0, nd.root()->separator().size());
LONGS_EQUAL(3, nd.root()->children().size()); // 4 leaf submaps
LONGS_EQUAL(1, nd.root()->weeklinks().size());
int numNodeStopPartition = 2;
int minNodesPerMap = 1;
NestedDissection<Graph, SubNLG, GenericGraph2D> nd(fg, ordering, numNodeStopPartition, minNodesPerMap);
LONGS_EQUAL(0, nd.root()->size()); // one weeklink
LONGS_EQUAL(1, nd.root()->frontal().size());
LONGS_EQUAL(0, nd.root()->separator().size());
LONGS_EQUAL(3, nd.root()->children().size()); // 4 leaf submaps
LONGS_EQUAL(1, nd.root()->weeklinks().size());
// the 1st submap
LONGS_EQUAL(2, nd.root()->children()[0]->frontal().size()); // x3 and x4
LONGS_EQUAL(4, nd.root()->children()[0]->size());
// the 1st submap
LONGS_EQUAL(2, nd.root()->children()[0]->frontal().size()); // x3 and x4
LONGS_EQUAL(4, nd.root()->children()[0]->size());
// the 2nd submap
LONGS_EQUAL(2, nd.root()->children()[1]->frontal().size()); // x1 and l6
LONGS_EQUAL(4, nd.root()->children()[1]->size());
//
// the 3rd submap
LONGS_EQUAL(1, nd.root()->children()[2]->frontal().size()); // x5
LONGS_EQUAL(1, nd.root()->children()[2]->size());
// the 2nd submap
LONGS_EQUAL(2, nd.root()->children()[1]->frontal().size()); // x1 and l6
LONGS_EQUAL(4, nd.root()->children()[1]->size());
//
// the 3rd submap
LONGS_EQUAL(1, nd.root()->children()[2]->frontal().size()); // x5
LONGS_EQUAL(1, nd.root()->children()[2]->size());
}
/* ************************************************************************* */
@ -184,86 +184,86 @@ TEST ( NestedDissection, weekLinks )
*/
TEST ( NestedDissection, manual_cuts )
{
using namespace submapPlanarSLAM;
typedef partition::Cuts Cuts;
typedef TSAM2D::SubNLG SubNLG;
typedef partition::PartitionTable PartitionTable;
Graph fg;
fg.addOdometry(x0, x1, Pose2(1.0, 0, 0), odoNoise);
fg.addOdometry(x1, x2, Pose2(1.0, 0, 0), odoNoise);
using namespace submapPlanarSLAM;
typedef partition::Cuts Cuts;
typedef TSAM2D::SubNLG SubNLG;
typedef partition::PartitionTable PartitionTable;
Graph fg;
fg.addOdometry(x0, x1, Pose2(1.0, 0, 0), odoNoise);
fg.addOdometry(x1, x2, Pose2(1.0, 0, 0), odoNoise);
fg.addBearingRange(x0, l1, Rot2::fromAngle( M_PI_2), 1, bearingRangeNoise);
fg.addBearingRange(x0, l4, Rot2::fromAngle(-M_PI_2), 1, bearingRangeNoise);
fg.addBearingRange(x0, l2, Rot2::fromAngle( M_PI_4), sqrt(2), bearingRangeNoise);
fg.addBearingRange(x0, l5, Rot2::fromAngle(-M_PI_4), sqrt(2), bearingRangeNoise);
fg.addBearingRange(x0, l1, Rot2::fromAngle( M_PI_2), 1, bearingRangeNoise);
fg.addBearingRange(x0, l4, Rot2::fromAngle(-M_PI_2), 1, bearingRangeNoise);
fg.addBearingRange(x0, l2, Rot2::fromAngle( M_PI_4), sqrt(2), bearingRangeNoise);
fg.addBearingRange(x0, l5, Rot2::fromAngle(-M_PI_4), sqrt(2), bearingRangeNoise);
fg.addBearingRange(x1, l1, Rot2::fromAngle( M_PI_4 * 3), sqrt(2), bearingRangeNoise);
fg.addBearingRange(x1, l2, Rot2::fromAngle( M_PI_2), 1, bearingRangeNoise);
fg.addBearingRange(x1, l3, Rot2::fromAngle( M_PI_4), sqrt(2), bearingRangeNoise);
fg.addBearingRange(x1, l4, Rot2::fromAngle(-M_PI_4 * 3), sqrt(2), bearingRangeNoise);
fg.addBearingRange(x1, l5, Rot2::fromAngle( M_PI_2), 1, bearingRangeNoise);
fg.addBearingRange(x1, l6, Rot2::fromAngle(-M_PI_4), sqrt(2), bearingRangeNoise);
fg.addBearingRange(x1, l1, Rot2::fromAngle( M_PI_4 * 3), sqrt(2), bearingRangeNoise);
fg.addBearingRange(x1, l2, Rot2::fromAngle( M_PI_2), 1, bearingRangeNoise);
fg.addBearingRange(x1, l3, Rot2::fromAngle( M_PI_4), sqrt(2), bearingRangeNoise);
fg.addBearingRange(x1, l4, Rot2::fromAngle(-M_PI_4 * 3), sqrt(2), bearingRangeNoise);
fg.addBearingRange(x1, l5, Rot2::fromAngle( M_PI_2), 1, bearingRangeNoise);
fg.addBearingRange(x1, l6, Rot2::fromAngle(-M_PI_4), sqrt(2), bearingRangeNoise);
fg.addBearingRange(x2, l2, Rot2::fromAngle( M_PI_4 * 3), sqrt(2), bearingRangeNoise);
fg.addBearingRange(x2, l5, Rot2::fromAngle(-M_PI_4 * 3), sqrt(2), bearingRangeNoise);
fg.addBearingRange(x2, l3, Rot2::fromAngle( M_PI_2), 1, bearingRangeNoise);
fg.addBearingRange(x2, l6, Rot2::fromAngle(-M_PI_2), 1, bearingRangeNoise);
fg.addBearingRange(x2, l2, Rot2::fromAngle( M_PI_4 * 3), sqrt(2), bearingRangeNoise);
fg.addBearingRange(x2, l5, Rot2::fromAngle(-M_PI_4 * 3), sqrt(2), bearingRangeNoise);
fg.addBearingRange(x2, l3, Rot2::fromAngle( M_PI_2), 1, bearingRangeNoise);
fg.addBearingRange(x2, l6, Rot2::fromAngle(-M_PI_2), 1, bearingRangeNoise);
fg.addPrior(x0, Pose2(0.1, 0, 0), priorNoise);
fg.addPrior(x0, Pose2(0.1, 0, 0), priorNoise);
// generate ordering
Ordering ordering; ordering += x0, x1, x2, l1, l2, l3, l4, l5, l6;
// generate ordering
Ordering ordering; ordering += x0, x1, x2, l1, l2, l3, l4, l5, l6;
// define cuts
boost::shared_ptr<Cuts> cuts(new Cuts());
cuts->partitionTable = PartitionTable(9, -1); PartitionTable* p = &cuts->partitionTable;
//x0 x1 x2 l1 l2 l3 l4 l5 l6
(*p)[0]=1; (*p)[1]=0; (*p)[2]=2; (*p)[3]=1; (*p)[4]=0; (*p)[5]=2; (*p)[6]=1; (*p)[7]=0; (*p)[8]=2;
// define cuts
boost::shared_ptr<Cuts> cuts(new Cuts());
cuts->partitionTable = PartitionTable(9, -1); PartitionTable* p = &cuts->partitionTable;
//x0 x1 x2 l1 l2 l3 l4 l5 l6
(*p)[0]=1; (*p)[1]=0; (*p)[2]=2; (*p)[3]=1; (*p)[4]=0; (*p)[5]=2; (*p)[6]=1; (*p)[7]=0; (*p)[8]=2;
cuts->children.push_back(boost::shared_ptr<Cuts>(new Cuts()));
cuts->children[0]->partitionTable = PartitionTable(9, -1); p = &cuts->children[0]->partitionTable;
//x0 x1 x2 l1 l2 l3 l4 l5 l6
(*p)[0]=0; (*p)[1]=-1; (*p)[2]=-1; (*p)[3]=1; (*p)[4]=-1; (*p)[5]=-1; (*p)[6]=2; (*p)[7]=-1; (*p)[8]=-1;
cuts->children.push_back(boost::shared_ptr<Cuts>(new Cuts()));
cuts->children[0]->partitionTable = PartitionTable(9, -1); p = &cuts->children[0]->partitionTable;
//x0 x1 x2 l1 l2 l3 l4 l5 l6
(*p)[0]=0; (*p)[1]=-1; (*p)[2]=-1; (*p)[3]=1; (*p)[4]=-1; (*p)[5]=-1; (*p)[6]=2; (*p)[7]=-1; (*p)[8]=-1;
cuts->children.push_back(boost::shared_ptr<Cuts>(new Cuts()));
cuts->children[1]->partitionTable = PartitionTable(9, -1); p = &cuts->children[1]->partitionTable;
//x0 x1 x2 l1 l2 l3 l4 l5 l6
(*p)[0]=-1; (*p)[1]=-1; (*p)[2]=0; (*p)[3]=-1; (*p)[4]=-1; (*p)[5]=1; (*p)[6]=-1; (*p)[7]=-1; (*p)[8]=2;
cuts->children.push_back(boost::shared_ptr<Cuts>(new Cuts()));
cuts->children[1]->partitionTable = PartitionTable(9, -1); p = &cuts->children[1]->partitionTable;
//x0 x1 x2 l1 l2 l3 l4 l5 l6
(*p)[0]=-1; (*p)[1]=-1; (*p)[2]=0; (*p)[3]=-1; (*p)[4]=-1; (*p)[5]=1; (*p)[6]=-1; (*p)[7]=-1; (*p)[8]=2;
// nested dissection
NestedDissection<Graph, SubNLG, GenericGraph2D> nd(fg, ordering, cuts);
LONGS_EQUAL(2, nd.root()->size());
LONGS_EQUAL(3, nd.root()->frontal().size());
LONGS_EQUAL(0, nd.root()->separator().size());
LONGS_EQUAL(2, nd.root()->children().size()); // 2 leaf submaps
LONGS_EQUAL(0, nd.root()->weeklinks().size());
// nested dissection
NestedDissection<Graph, SubNLG, GenericGraph2D> nd(fg, ordering, cuts);
LONGS_EQUAL(2, nd.root()->size());
LONGS_EQUAL(3, nd.root()->frontal().size());
LONGS_EQUAL(0, nd.root()->separator().size());
LONGS_EQUAL(2, nd.root()->children().size()); // 2 leaf submaps
LONGS_EQUAL(0, nd.root()->weeklinks().size());
// the 1st submap
LONGS_EQUAL(1, nd.root()->children()[0]->frontal().size()); // x0
LONGS_EQUAL(4, nd.root()->children()[0]->size());
LONGS_EQUAL(2, nd.root()->children()[0]->children().size());
// the 1st submap
LONGS_EQUAL(1, nd.root()->children()[0]->frontal().size()); // x0
LONGS_EQUAL(4, nd.root()->children()[0]->size());
LONGS_EQUAL(2, nd.root()->children()[0]->children().size());
// the 1-1st submap
LONGS_EQUAL(1, nd.root()->children()[0]->children()[0]->frontal().size()); // l1
LONGS_EQUAL(2, nd.root()->children()[0]->children()[0]->size());
// the 1-1st submap
LONGS_EQUAL(1, nd.root()->children()[0]->children()[0]->frontal().size()); // l1
LONGS_EQUAL(2, nd.root()->children()[0]->children()[0]->size());
// the 1-2nd submap
LONGS_EQUAL(1, nd.root()->children()[0]->children()[1]->frontal().size()); // l4
LONGS_EQUAL(2, nd.root()->children()[0]->children()[1]->size());
// the 1-2nd submap
LONGS_EQUAL(1, nd.root()->children()[0]->children()[1]->frontal().size()); // l4
LONGS_EQUAL(2, nd.root()->children()[0]->children()[1]->size());
// the 2nd submap
LONGS_EQUAL(1, nd.root()->children()[1]->frontal().size()); // x2
LONGS_EQUAL(3, nd.root()->children()[1]->size());
LONGS_EQUAL(2, nd.root()->children()[1]->children().size());
// the 2nd submap
LONGS_EQUAL(1, nd.root()->children()[1]->frontal().size()); // x2
LONGS_EQUAL(3, nd.root()->children()[1]->size());
LONGS_EQUAL(2, nd.root()->children()[1]->children().size());
// the 2-1st submap
LONGS_EQUAL(1, nd.root()->children()[1]->children()[0]->frontal().size()); // l3
LONGS_EQUAL(2, nd.root()->children()[1]->children()[0]->size());
// the 2-1st submap
LONGS_EQUAL(1, nd.root()->children()[1]->children()[0]->frontal().size()); // l3
LONGS_EQUAL(2, nd.root()->children()[1]->children()[0]->size());
// the 2-2nd submap
LONGS_EQUAL(1, nd.root()->children()[1]->children()[1]->frontal().size()); // l6
LONGS_EQUAL(2, nd.root()->children()[1]->children()[1]->size());
// the 2-2nd submap
LONGS_EQUAL(1, nd.root()->children()[1]->children()[1]->frontal().size()); // l6
LONGS_EQUAL(2, nd.root()->children()[1]->children()[1]->size());
}
@ -272,65 +272,65 @@ TEST ( NestedDissection, manual_cuts )
// / | / \ | \
// x0 x1 x2 x3
TEST( NestedDissection, Graph3D) {
using namespace gtsam::submapVisualSLAM;
typedef TSAM3D::SubNLG SubNLG;
typedef partition::PartitionTable PartitionTable;
vector<GeneralCamera> cameras;
cameras.push_back(GeneralCamera(Pose3(Rot3(), Point3(-2., 0., 0.))));
cameras.push_back(GeneralCamera(Pose3(Rot3(), Point3(-1., 0., 0.))));
cameras.push_back(GeneralCamera(Pose3(Rot3(), Point3( 1., 0., 0.))));
cameras.push_back(GeneralCamera(Pose3(Rot3(), Point3( 2., 0., 0.))));
using namespace gtsam::submapVisualSLAM;
typedef TSAM3D::SubNLG SubNLG;
typedef partition::PartitionTable PartitionTable;
vector<GeneralCamera> cameras;
cameras.push_back(GeneralCamera(Pose3(Rot3(), Point3(-2., 0., 0.))));
cameras.push_back(GeneralCamera(Pose3(Rot3(), Point3(-1., 0., 0.))));
cameras.push_back(GeneralCamera(Pose3(Rot3(), Point3( 1., 0., 0.))));
cameras.push_back(GeneralCamera(Pose3(Rot3(), Point3( 2., 0., 0.))));
vector<Point3> points;
for (int cube_index = 0; cube_index <= 3; cube_index++) {
Point3 center((cube_index-1) * 3, 0.5, 10.);
points.push_back(center + Point3(-0.5, -0.5, -0.5));
points.push_back(center + Point3(-0.5, 0.5, -0.5));
points.push_back(center + Point3( 0.5, 0.5, -0.5));
points.push_back(center + Point3( 0.5, -0.5, -0.5));
points.push_back(center + Point3(-0.5, -0.5, 0.5));
points.push_back(center + Point3(-0.5, 0.5, 0.5));
points.push_back(center + Point3( 0.5, 0.5, 0.5));
points.push_back(center + Point3( 0.5, 0.5, 0.5));
}
vector<Point3> points;
for (int cube_index = 0; cube_index <= 3; cube_index++) {
Point3 center((cube_index-1) * 3, 0.5, 10.);
points.push_back(center + Point3(-0.5, -0.5, -0.5));
points.push_back(center + Point3(-0.5, 0.5, -0.5));
points.push_back(center + Point3( 0.5, 0.5, -0.5));
points.push_back(center + Point3( 0.5, -0.5, -0.5));
points.push_back(center + Point3(-0.5, -0.5, 0.5));
points.push_back(center + Point3(-0.5, 0.5, 0.5));
points.push_back(center + Point3( 0.5, 0.5, 0.5));
points.push_back(center + Point3( 0.5, 0.5, 0.5));
}
Graph graph;
SharedDiagonal measurementNoise(gtsam::Vector_(2, 1., 1.));
SharedDiagonal measurementZeroNoise(gtsam::Vector_(2, 0., 0.));
for (int j=1; j<=8; j++)
graph.addMeasurement(0, j, cameras[0].project(points[j-1]).expmap(measurementZeroNoise->sample()), measurementNoise);
for (int j=1; j<=16; j++)
graph.addMeasurement(1, j, cameras[1].project(points[j-1]).expmap(measurementZeroNoise->sample()), measurementNoise);
for (int j=9; j<=24; j++)
graph.addMeasurement(2, j, cameras[2].project(points[j-1]).expmap(measurementZeroNoise->sample()), measurementNoise);
for (int j=17; j<=24; j++)
graph.addMeasurement(3, j, cameras[3].project(points[j-1]).expmap(measurementZeroNoise->sample()), measurementNoise);
Graph graph;
SharedDiagonal measurementNoise(gtsam::Vector_(2, 1., 1.));
SharedDiagonal measurementZeroNoise(gtsam::Vector_(2, 0., 0.));
for (int j=1; j<=8; j++)
graph.addMeasurement(0, j, cameras[0].project(points[j-1]).expmap(measurementZeroNoise->sample()), measurementNoise);
for (int j=1; j<=16; j++)
graph.addMeasurement(1, j, cameras[1].project(points[j-1]).expmap(measurementZeroNoise->sample()), measurementNoise);
for (int j=9; j<=24; j++)
graph.addMeasurement(2, j, cameras[2].project(points[j-1]).expmap(measurementZeroNoise->sample()), measurementNoise);
for (int j=17; j<=24; j++)
graph.addMeasurement(3, j, cameras[3].project(points[j-1]).expmap(measurementZeroNoise->sample()), measurementNoise);
// make an easy ordering
Ordering ordering; ordering += x0, x1, x2, x3;
for (int j=1; j<=24; j++)
ordering += Symbol('l', j);
// make an easy ordering
Ordering ordering; ordering += x0, x1, x2, x3;
for (int j=1; j<=24; j++)
ordering += Symbol('l', j);
// nested dissection
const int numNodeStopPartition = 10;
const int minNodesPerMap = 5;
NestedDissection<Graph, SubNLG, GenericGraph3D> nd(graph, ordering, numNodeStopPartition, minNodesPerMap);
// nested dissection
const int numNodeStopPartition = 10;
const int minNodesPerMap = 5;
NestedDissection<Graph, SubNLG, GenericGraph3D> nd(graph, ordering, numNodeStopPartition, minNodesPerMap);
LONGS_EQUAL(0, nd.root()->size());
LONGS_EQUAL(8, nd.root()->frontal().size()); // l9-l16
LONGS_EQUAL(0, nd.root()->separator().size());
LONGS_EQUAL(2, nd.root()->children().size()); // 2 leaf submaps
LONGS_EQUAL(0, nd.root()->weeklinks().size());
LONGS_EQUAL(0, nd.root()->size());
LONGS_EQUAL(8, nd.root()->frontal().size()); // l9-l16
LONGS_EQUAL(0, nd.root()->separator().size());
LONGS_EQUAL(2, nd.root()->children().size()); // 2 leaf submaps
LONGS_EQUAL(0, nd.root()->weeklinks().size());
// the 1st submap
LONGS_EQUAL(10, nd.root()->children()[0]->frontal().size()); // x0, x1, l1-l8
LONGS_EQUAL(24, nd.root()->children()[0]->size()); // 8 + 16
LONGS_EQUAL(0, nd.root()->children()[0]->children().size());
// the 1st submap
LONGS_EQUAL(10, nd.root()->children()[0]->frontal().size()); // x0, x1, l1-l8
LONGS_EQUAL(24, nd.root()->children()[0]->size()); // 8 + 16
LONGS_EQUAL(0, nd.root()->children()[0]->children().size());
// the 2nd submap
LONGS_EQUAL(10, nd.root()->children()[1]->frontal().size()); // x2, x3, l1-l8
LONGS_EQUAL(24, nd.root()->children()[1]->size()); // 16 + 8
LONGS_EQUAL(0, nd.root()->children()[1]->children().size());
// the 2nd submap
LONGS_EQUAL(10, nd.root()->children()[1]->frontal().size()); // x2, x3, l1-l8
LONGS_EQUAL(24, nd.root()->children()[1]->size()); // 16 + 8
LONGS_EQUAL(0, nd.root()->children()[1]->children().size());
}

100
gtsam_unstable/slam/BetweenFactorEM.h
View File

@ -295,87 +295,87 @@ namespace gtsam {
/* ************************************************************************* */
SharedGaussian get_model_inlier() const {
return model_inlier_;
return model_inlier_;
}
/* ************************************************************************* */
SharedGaussian get_model_outlier() const {
return model_outlier_;
return model_outlier_;
}
/* ************************************************************************* */
Matrix get_model_inlier_cov() const {
return (model_inlier_->R().transpose()*model_inlier_->R()).inverse();
return (model_inlier_->R().transpose()*model_inlier_->R()).inverse();
}
/* ************************************************************************* */
Matrix get_model_outlier_cov() const {
return (model_outlier_->R().transpose()*model_outlier_->R()).inverse();
return (model_outlier_->R().transpose()*model_outlier_->R()).inverse();
}
/* ************************************************************************* */
void updateNoiseModels(const gtsam::Values& values, const gtsam::NonlinearFactorGraph& graph){
/* Update model_inlier_ and model_outlier_ to account for uncertainty in robot trajectories
* (note these are given in the E step, where indicator probabilities are calculated).
*
* Principle: R += [H1 H2] * joint_cov12 * [H1 H2]', where H1, H2 are Jacobians of the
* unwhitened error w.r.t. states, and R is the measurement covariance (inlier or outlier modes).
*
* TODO: improve efficiency (info form)
*/
/* Update model_inlier_ and model_outlier_ to account for uncertainty in robot trajectories
* (note these are given in the E step, where indicator probabilities are calculated).
*
* Principle: R += [H1 H2] * joint_cov12 * [H1 H2]', where H1, H2 are Jacobians of the
* unwhitened error w.r.t. states, and R is the measurement covariance (inlier or outlier modes).
*
* TODO: improve efficiency (info form)
*/
// get joint covariance of the involved states
std::vector<gtsam::Key> Keys;
Keys.push_back(key1_);
Keys.push_back(key2_);
Marginals marginals( graph, values, Marginals::QR );
JointMarginal joint_marginal12 = marginals.jointMarginalCovariance(Keys);
Matrix cov1 = joint_marginal12(key1_, key1_);
Matrix cov2 = joint_marginal12(key2_, key2_);
Matrix cov12 = joint_marginal12(key1_, key2_);
// get joint covariance of the involved states
std::vector<gtsam::Key> Keys;
Keys.push_back(key1_);
Keys.push_back(key2_);
Marginals marginals( graph, values, Marginals::QR );
JointMarginal joint_marginal12 = marginals.jointMarginalCovariance(Keys);
Matrix cov1 = joint_marginal12(key1_, key1_);
Matrix cov2 = joint_marginal12(key2_, key2_);
Matrix cov12 = joint_marginal12(key1_, key2_);
updateNoiseModels_givenCovs(values, cov1, cov2, cov12);
updateNoiseModels_givenCovs(values, cov1, cov2, cov12);
}
/* ************************************************************************* */
void updateNoiseModels_givenCovs(const gtsam::Values& values, const Matrix& cov1, const Matrix& cov2, const Matrix& cov12){
/* Update model_inlier_ and model_outlier_ to account for uncertainty in robot trajectories
* (note these are given in the E step, where indicator probabilities are calculated).
*
* Principle: R += [H1 H2] * joint_cov12 * [H1 H2]', where H1, H2 are Jacobians of the
* unwhitened error w.r.t. states, and R is the measurement covariance (inlier or outlier modes).
*
* TODO: improve efficiency (info form)
*/
/* Update model_inlier_ and model_outlier_ to account for uncertainty in robot trajectories
* (note these are given in the E step, where indicator probabilities are calculated).
*
* Principle: R += [H1 H2] * joint_cov12 * [H1 H2]', where H1, H2 are Jacobians of the
* unwhitened error w.r.t. states, and R is the measurement covariance (inlier or outlier modes).
*
* TODO: improve efficiency (info form)
*/
const T& p1 = values.at<T>(key1_);
const T& p2 = values.at<T>(key2_);
const T& p1 = values.at<T>(key1_);
const T& p2 = values.at<T>(key2_);
Matrix H1, H2;
T hx = p1.between(p2, H1, H2); // h(x)
Matrix H1, H2;
T hx = p1.between(p2, H1, H2); // h(x)
Matrix H;
H.resize(H1.rows(), H1.rows()+H2.rows());
H << H1, H2; // H = [H1 H2]
Matrix H;
H.resize(H1.rows(), H1.rows()+H2.rows());
H << H1, H2; // H = [H1 H2]
Matrix joint_cov;
joint_cov.resize(cov1.rows()+cov2.rows(), cov1.cols()+cov2.cols());
joint_cov << cov1, cov12,
cov12.transpose(), cov2;
Matrix joint_cov;
joint_cov.resize(cov1.rows()+cov2.rows(), cov1.cols()+cov2.cols());
joint_cov << cov1, cov12,
cov12.transpose(), cov2;
Matrix cov_state = H*joint_cov*H.transpose();
Matrix cov_state = H*joint_cov*H.transpose();
// model_inlier_->print("before:");
// model_inlier_->print("before:");
// update inlier and outlier noise models
Matrix covRinlier = (model_inlier_->R().transpose()*model_inlier_->R()).inverse();
model_inlier_ = gtsam::noiseModel::Gaussian::Covariance(covRinlier + cov_state);
// update inlier and outlier noise models
Matrix covRinlier = (model_inlier_->R().transpose()*model_inlier_->R()).inverse();
model_inlier_ = gtsam::noiseModel::Gaussian::Covariance(covRinlier + cov_state);
Matrix covRoutlier = (model_outlier_->R().transpose()*model_outlier_->R()).inverse();
model_outlier_ = gtsam::noiseModel::Gaussian::Covariance(covRoutlier + cov_state);
Matrix covRoutlier = (model_outlier_->R().transpose()*model_outlier_->R()).inverse();
model_outlier_ = gtsam::noiseModel::Gaussian::Covariance(covRoutlier + cov_state);
// model_inlier_->print("after:");
// std::cout<<"covRinlier + cov_state: "<<covRinlier + cov_state<<std::endl;
// model_inlier_->print("after:");
// std::cout<<"covRinlier + cov_state: "<<covRinlier + cov_state<<std::endl;
}
/* ************************************************************************* */

105
gtsam_unstable/slam/BiasedGPSFactor.h Normal file
View File

@ -0,0 +1,105 @@
/* ----------------------------------------------------------------------------
* 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 BiasedGPSFactor.h
* @author Luca Carlone
**/
#pragma once
#include <ostream>
#include <gtsam/geometry/Pose3.h>
#include <gtsam/nonlinear/NonlinearFactor.h>
namespace gtsam {
/**
* A class to model GPS measurements, including a bias term which models
* common-mode errors and that can be partially corrected if other sensors are used
* @addtogroup SLAM
*/
class BiasedGPSFactor: public NoiseModelFactor2<Pose3, Point3> {
private:
typedef BiasedGPSFactor This;
typedef NoiseModelFactor2<Pose3, Point3> Base;
Point3 measured_; /** The measurement */
public:
// shorthand for a smart pointer to a factor
typedef boost::shared_ptr<BiasedGPSFactor> shared_ptr;
/** default constructor - only use for serialization */
BiasedGPSFactor() {}
/** Constructor */
BiasedGPSFactor(Key posekey, Key biaskey, const Point3 measured,
const SharedNoiseModel& model) :
Base(model, posekey, biaskey), measured_(measured) {
}
virtual ~BiasedGPSFactor() {}
/** implement functions needed for Testable */
/** print */
virtual void print(const std::string& s, const KeyFormatter& keyFormatter = DefaultKeyFormatter) const {
std::cout << s << "BiasedGPSFactor("
<< keyFormatter(this->key1()) << ","
<< keyFormatter(this->key2()) << ")\n";
measured_.print(" measured: ");
this->noiseModel_->print(" noise model: ");
}
/** equals */
virtual bool equals(const NonlinearFactor& expected, double tol=1e-9) const {
const This *e = dynamic_cast<const This*> (&expected);
return e != NULL && Base::equals(*e, tol) && this->measured_.equals(e->measured_, tol);
}
/** implement functions needed to derive from Factor */
/** vector of errors */
Vector evaluateError(const Pose3& pose, const Point3& bias,
boost::optional<Matrix&> H1 = boost::none, boost::optional<Matrix&> H2 =
boost::none) const {
if (H1 || H2){
H1->resize(3,6); // jacobian wrt pose
(*H1) << Matrix3::Zero(), pose.rotation().matrix();
H2->resize(3,3); // jacobian wrt bias
(*H2) << Matrix3::Identity();
}
return pose.translation().vector() + bias.vector() - measured_.vector();
}
/** return the measured */
const Point3 measured() const {
return measured_;
}
private:
/** Serialization function */
friend class boost::serialization::access;
template<class ARCHIVE>
void serialize(ARCHIVE & ar, const unsigned int version) {
ar & boost::serialization::make_nvp("NoiseModelFactor2",
boost::serialization::base_object<Base>(*this));
ar & BOOST_SERIALIZATION_NVP(measured_);
}
}; // \class BiasedGPSFactor
} /// namespace gtsam

135
gtsam_unstable/slam/GaussMarkov1stOrderFactor.h Normal file
View File

@ -0,0 +1,135 @@
/* ----------------------------------------------------------------------------
* 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 GaussMarkov1stOrderFactor.h
* @author Vadim Indelman, Stephen Williams, Luca Carlone
* @date Jan 17, 2012
**/
#pragma once
#include <ostream>
#include <gtsam/base/Testable.h>
#include <gtsam/base/Lie.h>
#include <gtsam/nonlinear/NonlinearFactor.h>
#include <gtsam/linear/GaussianFactor.h>
#include <gtsam/linear/NoiseModel.h>
namespace gtsam {
/*
* - The 1st order GaussMarkov factor relates two keys of the same type. This relation is given via
* key_2 = exp(-1/tau*delta_t) * key1 + w_d
* where tau is the time constant and delta_t is the time difference between the two keys.
* w_d is the equivalent discrete noise, whose covariance is calculated from the continuous noise model and delta_t.
* - w_d is approximated as a Gaussian noise.
* - In the multi-dimensional case, tau is a vector, and the above equation is applied on each element
* in the state (represented by keys), using the appropriate time constant in the vector tau.
*/
/*
* A class for a measurement predicted by "GaussMarkov1stOrderFactor(config[key1],config[key2])"
* KEY1::Value is the Lie Group type
* T is the measurement type, by default the same
*/
template<class VALUE>
class GaussMarkov1stOrderFactor: public NoiseModelFactor2<VALUE, VALUE> {
private:
typedef GaussMarkov1stOrderFactor<VALUE> This;
typedef NoiseModelFactor2<VALUE, VALUE> Base;
double dt_;
Vector tau_;
public:
// shorthand for a smart pointer to a factor
typedef typename boost::shared_ptr<GaussMarkov1stOrderFactor> shared_ptr;
/** default constructor - only use for serialization */
GaussMarkov1stOrderFactor() {}
/** Constructor */
GaussMarkov1stOrderFactor(const Key& key1, const Key& key2, double delta_t, Vector tau,
const SharedGaussian& model) :
Base(calcDiscreteNoiseModel(model, delta_t), key1, key2), dt_(delta_t), tau_(tau) {
}
virtual ~GaussMarkov1stOrderFactor() {}
/** implement functions needed for Testable */
/** print */
virtual void print(const std::string& s, const KeyFormatter& keyFormatter = DefaultKeyFormatter) const {
std::cout << s << "GaussMarkov1stOrderFactor("
<< keyFormatter(this->key1()) << ","
<< keyFormatter(this->key2()) << ")\n";
this->noiseModel_->print(" noise model");
}
/** equals */
virtual bool equals(const NonlinearFactor& expected, double tol=1e-9) const {
const This *e = dynamic_cast<const This*> (&expected);
return e != NULL && Base::equals(*e, tol);
}
/** implement functions needed to derive from Factor */
/** vector of errors */
Vector evaluateError(const VALUE& p1, const VALUE& p2,
boost::optional<Matrix&> H1 = boost::none,
boost::optional<Matrix&> H2 = boost::none) const {
Vector v1( VALUE::Logmap(p1) );
Vector v2( VALUE::Logmap(p2) );
Vector alpha(tau_.size());
Vector alpha_v1(tau_.size());
for(int i=0; i<tau_.size(); i++){
alpha(i) = exp(- 1/tau_(i)*dt_ );
alpha_v1(i) = alpha(i) * v1(i);
}
Vector hx(v2 - alpha_v1);
if(H1) *H1 = - diag(alpha);
if(H2) *H2 = eye(v2.size());
return hx;
}
private:
/** Serialization function */
friend class boost::serialization::access;
template<class ARCHIVE>
void serialize(ARCHIVE & ar, const unsigned int version) {
ar & BOOST_SERIALIZATION_BASE_OBJECT_NVP(Base);
ar & BOOST_SERIALIZATION_NVP(dt_);
ar & BOOST_SERIALIZATION_NVP(tau_);
}
SharedGaussian calcDiscreteNoiseModel(const SharedGaussian& model, double delta_t){
/* Q_d (approx)= Q * delta_t */
/* In practice, square root of the information matrix is represented, so that:
* R_d (approx)= R / sqrt(delta_t)
* */
noiseModel::Gaussian::shared_ptr gaussian_model = boost::dynamic_pointer_cast<noiseModel::Gaussian>(model);
SharedGaussian model_d(noiseModel::Gaussian::SqrtInformation(gaussian_model->R()/sqrt(delta_t)));
return model_d;
}
}; // \class GaussMarkov1stOrderFactor
} /// namespace gtsam

54
gtsam_unstable/slam/tests/testBetweenFactorEM.cpp
View File

@ -255,43 +255,43 @@ TEST( BetweenFactorEM, CaseStudy)
///* ************************************************************************** */
TEST (BetweenFactorEM, updateNoiseModel ) {
gtsam::Key key1(1);
gtsam::Key key2(2);
gtsam::Key key1(1);
gtsam::Key key2(2);
gtsam::Pose2 p1(10.0, 15.0, 0.1);
gtsam::Pose2 p2(15.0, 15.0, 0.3);
gtsam::Pose2 noise(0.5, 0.4, 0.01);
gtsam::Pose2 rel_pose_ideal = p1.between(p2);
gtsam::Pose2 rel_pose_msr = rel_pose_ideal.compose(noise);
gtsam::Pose2 p1(10.0, 15.0, 0.1);
gtsam::Pose2 p2(15.0, 15.0, 0.3);
gtsam::Pose2 noise(0.5, 0.4, 0.01);
gtsam::Pose2 rel_pose_ideal = p1.between(p2);
gtsam::Pose2 rel_pose_msr = rel_pose_ideal.compose(noise);
SharedGaussian model_inlier(noiseModel::Diagonal::Sigmas( (gtsam::Vector(3) << 1.5, 2.5, 4.05)));
SharedGaussian model_outlier(noiseModel::Diagonal::Sigmas( (gtsam::Vector(3) << 50.0, 50.0, 10.0)));
SharedGaussian model_inlier(noiseModel::Diagonal::Sigmas( (gtsam::Vector(3) << 1.5, 2.5, 4.05)));
SharedGaussian model_outlier(noiseModel::Diagonal::Sigmas( (gtsam::Vector(3) << 50.0, 50.0, 10.0)));
gtsam::Values values;
values.insert(key1, p1);
values.insert(key2, p2);
gtsam::Values values;
values.insert(key1, p1);
values.insert(key2, p2);
double prior_outlier = 0.0;
double prior_inlier = 1.0;
double prior_outlier = 0.0;
double prior_inlier = 1.0;
BetweenFactorEM<gtsam::Pose2> f(key1, key2, rel_pose_msr, model_inlier, model_outlier,
prior_inlier, prior_outlier);
BetweenFactorEM<gtsam::Pose2> f(key1, key2, rel_pose_msr, model_inlier, model_outlier,
prior_inlier, prior_outlier);
SharedGaussian model = SharedGaussian(noiseModel::Isotropic::Sigma(3, 1e2));
SharedGaussian model = SharedGaussian(noiseModel::Isotropic::Sigma(3, 1e2));
NonlinearFactorGraph graph;
graph.push_back(gtsam::PriorFactor<Pose2>(key1, p1, model));
graph.push_back(gtsam::PriorFactor<Pose2>(key2, p2, model));
NonlinearFactorGraph graph;
graph.push_back(gtsam::PriorFactor<Pose2>(key1, p1, model));
graph.push_back(gtsam::PriorFactor<Pose2>(key2, p2, model));
f.updateNoiseModels(values, graph);
f.updateNoiseModels(values, graph);
SharedGaussian model_inlier_new = f.get_model_inlier();
SharedGaussian model_outlier_new = f.get_model_outlier();
SharedGaussian model_inlier_new = f.get_model_inlier();
SharedGaussian model_outlier_new = f.get_model_outlier();
model_inlier->print("model_inlier:");
model_outlier->print("model_outlier:");
model_inlier_new->print("model_inlier_new:");
model_outlier_new->print("model_outlier_new:");
model_inlier->print("model_inlier:");
model_outlier->print("model_outlier:");
model_inlier_new->print("model_inlier_new:");
model_outlier_new->print("model_outlier_new:");
}

85
gtsam_unstable/slam/tests/testBiasedGPSFactor.cpp Normal file
View File

@ -0,0 +1,85 @@
/**
* @file testBiasedGPSFactor.cpp
* @brief
* @author Luca Carlone
* @date July 30, 2014
*/
#include <gtsam/base/numericalDerivative.h>
#include <gtsam/geometry/Pose3.h>
#include <gtsam/inference/Symbol.h>
#include <gtsam_unstable/slam/BiasedGPSFactor.h>
#include <CppUnitLite/TestHarness.h>
using namespace gtsam;
using namespace gtsam::symbol_shorthand;
using namespace gtsam::noiseModel;
// Convenience for named keys
using symbol_shorthand::X;
using symbol_shorthand::B;
TEST(BiasedGPSFactor, errorNoiseless) {
Rot3 R = Rot3::rodriguez(0.1, 0.2, 0.3);
Point3 t(1.0, 0.5, 0.2);
Pose3 pose(R,t);
Point3 bias(0.0,0.0,0.0);
Point3 noise(0.0,0.0,0.0);
Point3 measured = t + noise;
BiasedGPSFactor factor(X(1), B(1), measured, Isotropic::Sigma(3, 0.05));
Vector expectedError = (Vector(3) << 0.0, 0.0, 0.0 );
Vector actualError = factor.evaluateError(pose,bias);
EXPECT(assert_equal(expectedError,actualError, 1E-5));
}
TEST(BiasedGPSFactor, errorNoisy) {
Rot3 R = Rot3::rodriguez(0.1, 0.2, 0.3);
Point3 t(1.0, 0.5, 0.2);
Pose3 pose(R,t);
Point3 bias(0.0,0.0,0.0);
Point3 noise(1.0,2.0,3.0);
Point3 measured = t - noise;
BiasedGPSFactor factor(X(1), B(1), measured, Isotropic::Sigma(3, 0.05));
Vector expectedError = (Vector(3) << 1.0, 2.0, 3.0 );
Vector actualError = factor.evaluateError(pose,bias);
EXPECT(assert_equal(expectedError,actualError, 1E-5));
}
TEST(BiasedGPSFactor, jacobian) {
Rot3 R = Rot3::rodriguez(0.1, 0.2, 0.3);
Point3 t(1.0, 0.5, 0.2);
Pose3 pose(R,t);
Point3 bias(0.0,0.0,0.0);
Point3 noise(0.0,0.0,0.0);
Point3 measured = t + noise;
BiasedGPSFactor factor(X(1), B(1), measured, Isotropic::Sigma(3, 0.05));
Matrix actualH1, actualH2;
factor.evaluateError(pose,bias, actualH1, actualH2);
Matrix numericalH1 = numericalDerivative21(
boost::function<Vector(const Pose3&, const Point3&)>(boost::bind(
&BiasedGPSFactor::evaluateError, factor, _1, _2, boost::none,
boost::none)), pose, bias, 1e-5);
EXPECT(assert_equal(numericalH1,actualH1, 1E-5));
Matrix numericalH2 = numericalDerivative22(
boost::function<Vector(const Pose3&, const Point3&)>(boost::bind(
&BiasedGPSFactor::evaluateError, factor, _1, _2, boost::none,
boost::none)), pose, bias, 1e-5);
EXPECT(assert_equal(numericalH2,actualH2, 1E-5));
}
/* ************************************************************************* */
int main() {
TestResult tr;
return TestRegistry::runAllTests(tr);
}
/* ************************************************************************* */

124
gtsam_unstable/slam/tests/testGaussMarkov1stOrderFactor.cpp Normal file
View File

@ -0,0 +1,124 @@
/* ----------------------------------------------------------------------------
* 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 testGaussMarkov1stOrderFactor.cpp
* @brief Unit tests for the GaussMarkov1stOrder factor
* @author Vadim Indelman
* @date Jan 17, 2012
*/
#include <gtsam_unstable/slam/GaussMarkov1stOrderFactor.h>
#include <gtsam/nonlinear/Values.h>
#include <gtsam/inference/Key.h>
#include <gtsam/base/numericalDerivative.h>
#include <gtsam/base/LieVector.h>
#include <CppUnitLite/TestHarness.h>
using namespace std;
using namespace gtsam;
//! Factors
typedef GaussMarkov1stOrderFactor<LieVector> GaussMarkovFactor;
/* ************************************************************************* */
LieVector predictionError(const LieVector& v1, const LieVector& v2, const GaussMarkovFactor factor) {
return factor.evaluateError(v1, v2);
}
/* ************************************************************************* */
TEST( GaussMarkovFactor, equals )
{
// Create two identical factors and make sure they're equal
Key x1(1);
Key x2(2);
double delta_t = 0.10;
Vector tau = (Vector(3) << 100.0, 150.0, 10.0);
SharedGaussian model = noiseModel::Isotropic::Sigma(3, 1.0);
GaussMarkovFactor factor1(x1, x2, delta_t, tau, model);
GaussMarkovFactor factor2(x1, x2, delta_t, tau, model);
CHECK(assert_equal(factor1, factor2));
}
/* ************************************************************************* */
TEST( GaussMarkovFactor, error )
{
Values linPoint;
Key x1(1);
Key x2(2);
double delta_t = 0.10;
Vector tau = (Vector(3) << 100.0, 150.0, 10.0);
SharedGaussian model = noiseModel::Isotropic::Sigma(3, 1.0);
LieVector v1 = LieVector((Vector(3) << 10.0, 12.0, 13.0));
LieVector v2 = LieVector((Vector(3) << 10.0, 15.0, 14.0));
// Create two nodes
linPoint.insert(x1, v1);
linPoint.insert(x2, v2);
GaussMarkovFactor factor(x1, x2, delta_t, tau, model);
Vector Err1( factor.evaluateError(v1, v2) );
// Manually calculate the error
Vector alpha(tau.size());
Vector alpha_v1(tau.size());
for(int i=0; i<tau.size(); i++){
alpha(i) = exp(- 1/tau(i)*delta_t );
alpha_v1(i) = alpha(i) * v1(i);
}
Vector Err2( v2 - alpha_v1 );
CHECK(assert_equal(Err1, Err2, 1e-9));
}
/* ************************************************************************* */
TEST (GaussMarkovFactor, jacobian ) {
Values linPoint;
Key x1(1);
Key x2(2);
double delta_t = 0.10;
Vector tau = (Vector(3) << 100.0, 150.0, 10.0);
SharedGaussian model = noiseModel::Isotropic::Sigma(3, 1.0);
GaussMarkovFactor factor(x1, x2, delta_t, tau, model);
// Update the linearization point
LieVector v1_upd = LieVector((Vector(3) << 0.5, -0.7, 0.3));
LieVector v2_upd = LieVector((Vector(3) << -0.7, 0.4, 0.9));
// Calculate the Jacobian matrix using the factor
Matrix computed_H1, computed_H2;
factor.evaluateError(v1_upd, v2_upd, computed_H1, computed_H2);
// Calculate the Jacobian matrices H1 and H2 using the numerical derivative function
Matrix numerical_H1, numerical_H2;
numerical_H1 = numericalDerivative21<Vector3, Vector3, Vector3>(
boost::bind(&predictionError, _1, _2, factor), v1_upd, v2_upd);
numerical_H2 = numericalDerivative22<Vector3, Vector3, Vector3>(
boost::bind(&predictionError, _1, _2, factor), v1_upd, v2_upd);
// Verify they are equal for this choice of state
CHECK( assert_equal(numerical_H1, computed_H1, 1e-9));
CHECK( assert_equal(numerical_H2, computed_H2, 1e-9));
}
/* ************************************************************************* */
int main()
{
TestResult tr; return TestRegistry::runAllTests(tr);
}
/* ************************************************************************* */

26
timing/timePinholeCamera.cpp
View File

@ -26,7 +26,7 @@ using namespace gtsam;
int main()
{
int n = 1000000;
int n = 1e6;
const Pose3 pose1((Matrix)(Matrix(3,3) <<
1., 0., 0.,
@ -35,8 +35,6 @@ int main()
),
Point3(0,0,0.5));
// static Cal3_S2 K(500, 100, 0.1, 320, 240);
// static Cal3DS2 K(500, 100, 0.1, 320, 240, 1e-3, 2.0*1e-3, 3.0*1e-3, 4.0*1e-3);
static Cal3Bundler K(500, 1e-3, 2.0*1e-3);
const PinholeCamera<Cal3Bundler> camera(pose1,K);
const Point3 point1(-0.08,-0.08, 0.0);
@ -63,8 +61,18 @@ int main()
camera.project(point1);
long timeLog2 = clock();
double seconds = (double)(timeLog2-timeLog)/CLOCKS_PER_SEC;
cout << ((double)n/seconds) << " calls/second" << endl;
cout << ((double)seconds*1000000/n) << " musecs/call" << endl;
cout << ((double)seconds*1e9/n) << " nanosecs/call" << endl;
}
// Oct 12 2014, Macbook Air
{
long timeLog = clock();
Point2 measurement(0,0);
for(int i = 0; i < n; i++)
measurement.localCoordinates(camera.project(point1));
long timeLog2 = clock();
double seconds = (double)(timeLog2-timeLog)/CLOCKS_PER_SEC;
cout << ((double)seconds*1e9/n) << " nanosecs/call" << endl;
}
// Oct 12 2013, iMac 3.06GHz Core i3
@ -84,8 +92,7 @@ int main()
camera.project(point1, Dpose, Dpoint);
long timeLog2 = clock();
double seconds = (double)(timeLog2-timeLog)/CLOCKS_PER_SEC;
cout << ((double)n/seconds) << " calls/second" << endl;
cout << ((double)seconds*1000000/n) << " musecs/call" << endl;
cout << ((double)seconds*1e9/n) << " nanosecs/call" << endl;
}
// Oct 12 2013, iMac 3.06GHz Core i3
@ -97,7 +104,7 @@ int main()
// Cal3Bundler fix: 2.0946 musecs/call
// June 24 2014, Macbook Pro 2.3GHz Core i7
// GTSAM 3.1: 0.2294 musecs/call
// After project fix: 0.2093 musecs/call
// After project fix: 0.2093 nanosecs/call
{
Matrix Dpose, Dpoint, Dcal;
long timeLog = clock();
@ -105,8 +112,7 @@ int main()
camera.project(point1, Dpose, Dpoint, Dcal);
long timeLog2 = clock();
double seconds = (double)(timeLog2-timeLog)/CLOCKS_PER_SEC;
cout << ((double)n/seconds) << " calls/second" << endl;
cout << ((double)seconds*1000000/n) << " musecs/call" << endl;
cout << ((double)seconds*1e9/n) << " nanosecs/call" << endl;
}
return 0;