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>

1
.gitignore vendored
View File

@ -4,3 +4,4 @@
*.DS_Store
/examples/Data/dubrovnik-3-7-pre-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)

8
cmake/FindMKL.cmake
View File

@ -138,12 +138,15 @@ ELSE() # UNIX and macOS
${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()
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;
}

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

16
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,18 +40,16 @@ 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 ;
@ -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/nonlinear/NonlinearOptimizer.cpp
View File

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

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

41
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]);
//
// 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;
// }
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));
}

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

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

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;