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

Merge branch 'develop' into feature/discrete_wrapper

release/4.3a0
Frank Dellaert 2021-12-06 13:22:25 -05:00
parent f50f963e57 c7781961e6
commit 7891154d8e
201 changed files with 12536 additions and 4308 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

1
.gitignore vendored
View File

@ -3,6 +3,7 @@
.idea .idea
*.pyc *.pyc
*.DS_Store *.DS_Store
*.swp
/examples/Data/dubrovnik-3-7-pre-rewritten.txt /examples/Data/dubrovnik-3-7-pre-rewritten.txt
/examples/Data/pose2example-rewritten.txt /examples/Data/pose2example-rewritten.txt
/examples/Data/pose3example-rewritten.txt /examples/Data/pose3example-rewritten.txt

5
CMakeLists.txt
View File

@ -105,6 +105,11 @@ endif()
GtsamMakeConfigFile(GTSAM "${CMAKE_CURRENT_SOURCE_DIR}/gtsam_extra.cmake.in") GtsamMakeConfigFile(GTSAM "${CMAKE_CURRENT_SOURCE_DIR}/gtsam_extra.cmake.in")
export(TARGETS ${GTSAM_EXPORTED_TARGETS} FILE GTSAM-exports.cmake) export(TARGETS ${GTSAM_EXPORTED_TARGETS} FILE GTSAM-exports.cmake)
if (GTSAM_BUILD_UNSTABLE)
GtsamMakeConfigFile(GTSAM_UNSTABLE "${CMAKE_CURRENT_SOURCE_DIR}/gtsam_extra.cmake.in")
export(TARGETS ${GTSAM_UNSTABLE_EXPORTED_TARGETS} FILE GTSAM_UNSTABLE-exports.cmake)
endif()
# Check for doxygen availability - optional dependency # Check for doxygen availability - optional dependency
find_package(Doxygen) find_package(Doxygen)

2
cmake/GtsamMakeConfigFile.cmake
View File

@ -27,6 +27,8 @@ function(GtsamMakeConfigFile PACKAGE_NAME)
# here. # here.
if(NOT DEFINED ${PACKAGE_NAME}_VERSION AND DEFINED ${PACKAGE_NAME}_VERSION_STRING) if(NOT DEFINED ${PACKAGE_NAME}_VERSION AND DEFINED ${PACKAGE_NAME}_VERSION_STRING)
set(${PACKAGE_NAME}_VERSION ${${PACKAGE_NAME}_VERSION_STRING}) set(${PACKAGE_NAME}_VERSION ${${PACKAGE_NAME}_VERSION_STRING})
elseif(NOT DEFINED ${PACKAGE_NAME}_VERSION_STRING)
set(${PACKAGE_NAME}_VERSION ${GTSAM_VERSION_STRING})
endif() endif()
# Version file # Version file

29
cmake/HandleGeneralOptions.cmake
View File

@ -14,20 +14,21 @@ if(GTSAM_UNSTABLE_AVAILABLE)
option(GTSAM_UNSTABLE_BUILD_PYTHON "Enable/Disable Python wrapper for libgtsam_unstable" ON) option(GTSAM_UNSTABLE_BUILD_PYTHON "Enable/Disable Python wrapper for libgtsam_unstable" ON)
option(GTSAM_UNSTABLE_INSTALL_MATLAB_TOOLBOX "Enable/Disable MATLAB wrapper for libgtsam_unstable" OFF) option(GTSAM_UNSTABLE_INSTALL_MATLAB_TOOLBOX "Enable/Disable MATLAB wrapper for libgtsam_unstable" OFF)
endif() endif()
option(BUILD_SHARED_LIBS "Build shared gtsam library, instead of static" ON) option(BUILD_SHARED_LIBS "Build shared gtsam library, instead of static" ON)
option(GTSAM_USE_QUATERNIONS "Enable/Disable using an internal Quaternion representation for rotations instead of rotation matrices. If enable, Rot3::EXPMAP is enforced by default." OFF) option(GTSAM_USE_QUATERNIONS "Enable/Disable using an internal Quaternion representation for rotations instead of rotation matrices. If enable, Rot3::EXPMAP is enforced by default." OFF)
option(GTSAM_POSE3_EXPMAP "Enable/Disable using Pose3::EXPMAP as the default mode. If disabled, Pose3::FIRST_ORDER will be used." ON) option(GTSAM_POSE3_EXPMAP "Enable/Disable using Pose3::EXPMAP as the default mode. If disabled, Pose3::FIRST_ORDER will be used." ON)
option(GTSAM_ROT3_EXPMAP "Ignore if GTSAM_USE_QUATERNIONS is OFF (Rot3::EXPMAP by default). Otherwise, enable Rot3::EXPMAP, or if disabled, use Rot3::CAYLEY." ON) option(GTSAM_ROT3_EXPMAP "Ignore if GTSAM_USE_QUATERNIONS is OFF (Rot3::EXPMAP by default). Otherwise, enable Rot3::EXPMAP, or if disabled, use Rot3::CAYLEY." ON)
option(GTSAM_ENABLE_CONSISTENCY_CHECKS "Enable/Disable expensive consistency checks" OFF) option(GTSAM_ENABLE_CONSISTENCY_CHECKS "Enable/Disable expensive consistency checks" OFF)
option(GTSAM_WITH_TBB "Use Intel Threaded Building Blocks (TBB) if available" ON) option(GTSAM_WITH_TBB "Use Intel Threaded Building Blocks (TBB) if available" ON)
option(GTSAM_WITH_EIGEN_MKL "Eigen will use Intel MKL if available" OFF) option(GTSAM_WITH_EIGEN_MKL "Eigen will use Intel MKL if available" OFF)
option(GTSAM_WITH_EIGEN_MKL_OPENMP "Eigen, when using Intel MKL, will also use OpenMP for multithreading if available" OFF) option(GTSAM_WITH_EIGEN_MKL_OPENMP "Eigen, when using Intel MKL, will also use OpenMP for multithreading if available" OFF)
option(GTSAM_THROW_CHEIRALITY_EXCEPTION "Throw exception when a triangulated point is behind a camera" ON) option(GTSAM_THROW_CHEIRALITY_EXCEPTION "Throw exception when a triangulated point is behind a camera" ON)
option(GTSAM_BUILD_PYTHON "Enable/Disable building & installation of Python module with pybind11" OFF) option(GTSAM_BUILD_PYTHON "Enable/Disable building & installation of Python module with pybind11" OFF)
option(GTSAM_INSTALL_MATLAB_TOOLBOX "Enable/Disable installation of matlab toolbox" OFF) option(GTSAM_INSTALL_MATLAB_TOOLBOX "Enable/Disable installation of matlab toolbox" OFF)
option(GTSAM_ALLOW_DEPRECATED_SINCE_V41 "Allow use of methods/functions deprecated in GTSAM 4.1" ON) option(GTSAM_ALLOW_DEPRECATED_SINCE_V41 "Allow use of methods/functions deprecated in GTSAM 4.1" ON)
option(GTSAM_SUPPORT_NESTED_DISSECTION "Support Metis-based nested dissection" ON) option(GTSAM_SUPPORT_NESTED_DISSECTION "Support Metis-based nested dissection" ON)
option(GTSAM_TANGENT_PREINTEGRATION "Use new ImuFactor with integration on tangent space" ON) option(GTSAM_TANGENT_PREINTEGRATION "Use new ImuFactor with integration on tangent space" ON)
option(GTSAM_SLOW_BUT_CORRECT_BETWEENFACTOR "Use the slower but correct version of BetweenFactor" OFF)
if(NOT MSVC AND NOT XCODE_VERSION) if(NOT MSVC AND NOT XCODE_VERSION)
option(GTSAM_BUILD_WITH_CCACHE "Use ccache compiler cache" ON) option(GTSAM_BUILD_WITH_CCACHE "Use ccache compiler cache" ON)
endif() endif()

6
cmake/HandleTBB.cmake
View File

@ -7,9 +7,9 @@ if (GTSAM_WITH_TBB)
if(TBB_FOUND) if(TBB_FOUND)
set(GTSAM_USE_TBB 1) # This will go into config.h set(GTSAM_USE_TBB 1) # This will go into config.h
if ((${TBB_VERSION} VERSION_GREATER "2021.1") OR (${TBB_VERSION} VERSION_EQUAL "2021.1")) # if ((${TBB_VERSION} VERSION_GREATER "2021.1") OR (${TBB_VERSION} VERSION_EQUAL "2021.1"))
message(FATAL_ERROR "TBB version greater than 2021.1 (oneTBB API) is not yet supported. Use an older version instead.") # message(FATAL_ERROR "TBB version greater than 2021.1 (oneTBB API) is not yet supported. Use an older version instead.")
endif() # endif()
if ((${TBB_VERSION_MAJOR} GREATER 2020) OR (${TBB_VERSION_MAJOR} EQUAL 2020)) if ((${TBB_VERSION_MAJOR} GREATER 2020) OR (${TBB_VERSION_MAJOR} EQUAL 2020))
set(TBB_GREATER_EQUAL_2020 1) set(TBB_GREATER_EQUAL_2020 1)

388
doc/math.lyx
View File

@ -5082,6 +5082,394 @@ reference "ex:projection"
\end_inset \end_inset
\end_layout
\begin_layout Subsection
Derivative of Adjoint
\begin_inset CommandInset label
LatexCommand label
name "subsec:pose3_adjoint_deriv"
\end_inset
\end_layout
\begin_layout Standard
Consider
\begin_inset Formula $f:SE(3)\times\mathbb{R}^{6}\rightarrow\mathbb{R}^{6}$
\end_inset
is defined as
\begin_inset Formula $f(T,\xi_{b})=Ad_{T}\hat{\xi}_{b}$
\end_inset
.
The derivative is notated (see Section
\begin_inset CommandInset ref
LatexCommand ref
reference "sec:Derivatives-of-Actions"
plural "false"
caps "false"
noprefix "false"
\end_inset
):
\end_layout
\begin_layout Standard
\begin_inset Formula
\[
Df_{(T,\xi_{b})}(\xi,\delta\xi_{b})=D_{1}f_{(T,\xi_{b})}(\xi)+D_{2}f_{(T,\xi_{b})}(\delta\xi_{b})
\]
\end_inset
First, computing
\begin_inset Formula $D_{2}f_{(T,\xi_{b})}(\xi_{b})$
\end_inset
is easy, as its matrix is simply
\begin_inset Formula $Ad_{T}$
\end_inset
:
\end_layout
\begin_layout Standard
\begin_inset Formula
\[
f(T,\xi_{b}+\delta\xi_{b})=Ad_{T}(\widehat{\xi_{b}+\delta\xi_{b}})=Ad_{T}(\hat{\xi}_{b})+Ad_{T}(\delta\hat{\xi}_{b})
\]
\end_inset
\end_layout
\begin_layout Standard
\begin_inset Formula
\[
D_{2}f_{(T,\xi_{b})}(\xi_{b})=Ad_{T}
\]
\end_inset
We will derive
\begin_inset Formula $D_{1}f_{(T,\xi_{b})}(\xi)$
\end_inset
using two approaches.
In the first, we'll define
\begin_inset Formula $g(T,\xi)\triangleq T\exp\hat{\xi}$
\end_inset
.
From Section
\begin_inset CommandInset ref
LatexCommand ref
reference "sec:Derivatives-of-Actions"
plural "false"
caps "false"
noprefix "false"
\end_inset
,
\end_layout
\begin_layout Standard
\begin_inset Formula
\begin{align*}
D_{2}g_{(T,\xi)}(\xi) & =T\hat{\xi}\\
D_{2}g_{(T,\xi)}^{-1}(\xi) & =-\hat{\xi}T^{-1}
\end{align*}
\end_inset
Now we can use the definition of the Adjoint representation
\begin_inset Formula $Ad_{g}\hat{\xi}=g\hat{\xi}g^{-1}$
\end_inset
(aka conjugation by
\begin_inset Formula $g$
\end_inset
) then apply product rule and simplify:
\end_layout
\begin_layout Standard
\begin_inset Formula
\begin{align*}
D_{1}f_{(T,\xi_{b})}(\xi)=D_{1}\left(Ad_{T\exp(\hat{\xi})}\hat{\xi}_{b}\right)(\xi) & =D_{1}\left(g\hat{\xi}_{b}g^{-1}\right)(\xi)\\
& =\left(D_{2}g_{(T,\xi)}(\xi)\right)\hat{\xi}_{b}g^{-1}(T,0)+g(T,0)\hat{\xi}_{b}\left(D_{2}g_{(T,\xi)}^{-1}(\xi)\right)\\
& =T\hat{\xi}\hat{\xi}_{b}T^{-1}-T\hat{\xi}_{b}\hat{\xi}T^{-1}\\
& =T\left(\hat{\xi}\hat{\xi}_{b}-\hat{\xi}_{b}\hat{\xi}\right)T^{-1}\\
& =Ad_{T}(ad_{\hat{\xi}}\hat{\xi}_{b})\\
& =-Ad_{T}(ad_{\hat{\xi}_{b}}\hat{\xi})\\
D_{1}F_{(T,\xi_{b})} & =-(Ad_{T})(ad_{\hat{\xi}_{b}})
\end{align*}
\end_inset
Where
\begin_inset Formula $ad_{\hat{\xi}}:\mathfrak{g}\rightarrow\mathfrak{g}$
\end_inset
is the adjoint map of the lie algebra.
\end_layout
\begin_layout Standard
The second, perhaps more intuitive way of deriving
\begin_inset Formula $D_{1}f_{(T,\xi_{b})}(\xi_{b})$
\end_inset
, would be to use the fact that the derivative at the origin
\begin_inset Formula $D_{1}Ad_{I}\hat{\xi}_{b}=ad_{\hat{\xi}_{b}}$
\end_inset
by definition of the adjoint
\begin_inset Formula $ad_{\xi}$
\end_inset
.
Then applying the property
\begin_inset Formula $Ad_{AB}=Ad_{A}Ad_{B}$
\end_inset
,
\end_layout
\begin_layout Standard
\begin_inset Formula
\[
D_{1}Ad_{T}\hat{\xi}_{b}(\xi)=D_{1}Ad_{T*I}\hat{\xi}_{b}(\xi)=Ad_{T}\left(D_{1}Ad_{I}\hat{\xi}_{b}(\xi)\right)=Ad_{T}\left(ad_{\hat{\xi}}(\hat{\xi}_{b})\right)=-Ad_{T}\left(ad_{\hat{\xi}_{b}}(\hat{\xi})\right)
\]
\end_inset
\end_layout
\begin_layout Subsection
Derivative of AdjointTranspose
\end_layout
\begin_layout Standard
The transpose of the Adjoint,
\family roman
\series medium
\shape up
\size normal
\emph off
\bar no
\strikeout off
\xout off
\uuline off
\uwave off
\noun off
\color none
\begin_inset Formula $Ad_{T}^{T}:\mathfrak{g^{*}\rightarrow g^{*}}$
\end_inset
, is useful as a way to change the reference frame of vectors in the dual
space
\family default
\series default
\shape default
\size default
\emph default
\bar default
\strikeout default
\xout default
\uuline default
\uwave default
\noun default
\color inherit
(note the
\begin_inset Formula $^{*}$
\end_inset
denoting that we are now in the dual space)
\family roman
\series medium
\shape up
\size normal
\emph off
\bar no
\strikeout off
\xout off
\uuline off
\uwave off
\noun off
\color none
.
To be more concrete, where
\family default
\series default
\shape default
\size default
\emph default
\bar default
\strikeout default
\xout default
\uuline default
\uwave default
\noun default
\color inherit
as
\begin_inset Formula $Ad_{T}\hat{\xi}_{b}$
\end_inset
converts the
\emph on
twist
\emph default
\family roman
\series medium
\shape up
\size normal
\emph off
\bar no
\strikeout off
\xout off
\uuline off
\uwave off
\noun off
\color none
\begin_inset Formula $\xi_{b}$
\end_inset
from the
\begin_inset Formula $T$
\end_inset
frame,
\family default
\series default
\shape default
\size default
\emph default
\bar default
\strikeout default
\xout default
\uuline default
\uwave default
\noun default
\color inherit
\family roman
\series medium
\shape up
\size normal
\emph off
\bar no
\strikeout off
\xout off
\uuline off
\uwave off
\noun off
\color none
\begin_inset Formula $Ad_{T}^{T}\hat{\xi}_{b}^{*}$
\end_inset
converts the
\family default
\series default
\shape default
\size default
\emph on
\bar default
\strikeout default
\xout default
\uuline default
\uwave default
\noun default
\color inherit
wrench
\emph default
\family roman
\series medium
\shape up
\size normal
\emph off
\bar no
\strikeout off
\xout off
\uuline off
\uwave off
\noun off
\color none
\begin_inset Formula $\xi_{b}^{*}$
\end_inset
from the
\begin_inset Formula $T$
\end_inset
frame
\family default
\series default
\shape default
\size default
\emph default
\bar default
\strikeout default
\xout default
\uuline default
\uwave default
\noun default
\color inherit
.
It's difficult to apply a similar derivation as in Section
\begin_inset CommandInset ref
LatexCommand ref
reference "subsec:pose3_adjoint_deriv"
plural "false"
caps "false"
noprefix "false"
\end_inset
for the derivative of
\begin_inset Formula $Ad_{T}^{T}\hat{\xi}_{b}^{*}$
\end_inset
because
\begin_inset Formula $Ad_{T}^{T}$
\end_inset
cannot be naturally defined as a conjugation, so we resort to crunching
through the algebra.
The details are omitted but the result is a form that vaguely resembles
(but does not exactly match)
\begin_inset Formula $ad(Ad_{T}^{T}\hat{\xi}_{b}^{*})$
\end_inset
:
\end_layout
\begin_layout Standard
\begin_inset Formula
\begin{align*}
\begin{bmatrix}\omega_{T}\\
v_{T}
\end{bmatrix}^{*} & \triangleq Ad_{T}^{T}\hat{\xi}_{b}^{*}\\
D_{1}Ad_{T}^{T}\hat{\xi}_{b}^{*}(\xi) & =\begin{bmatrix}\hat{\omega}_{T} & \hat{v}_{T}\\
\hat{v}_{T} & 0
\end{bmatrix}
\end{align*}
\end_inset
\end_layout \end_layout
\begin_layout Subsection \begin_layout Subsection

BIN
doc/math.pdf
View File

Binary file not shown.

12
examples/DiscreteBayesNetExample.cpp
View File

@ -56,8 +56,8 @@ int main(int argc, char **argv) {
DiscreteBayesNet::shared_ptr chordal = fg.eliminateSequential(ordering); DiscreteBayesNet::shared_ptr chordal = fg.eliminateSequential(ordering);
// solve // solve
DiscreteFactor::sharedValues mpe = chordal->optimize(); auto mpe = chordal->optimize();
GTSAM_PRINT(*mpe); GTSAM_PRINT(mpe);
// We can also build a Bayes tree (directed junction tree). // We can also build a Bayes tree (directed junction tree).
// The elimination order above will do fine: // The elimination order above will do fine:
@ -70,14 +70,14 @@ int main(int argc, char **argv) {
// solve again, now with evidence // solve again, now with evidence
DiscreteBayesNet::shared_ptr chordal2 = fg.eliminateSequential(ordering); DiscreteBayesNet::shared_ptr chordal2 = fg.eliminateSequential(ordering);
DiscreteFactor::sharedValues mpe2 = chordal2->optimize(); auto mpe2 = chordal2->optimize();
GTSAM_PRINT(*mpe2); GTSAM_PRINT(mpe2);
// We can also sample from it // We can also sample from it
cout << "\n10 samples:" << endl; cout << "\n10 samples:" << endl;
for (size_t i = 0; i < 10; i++) { for (size_t i = 0; i < 10; i++) {
DiscreteFactor::sharedValues sample = chordal2->sample(); auto sample = chordal2->sample();
GTSAM_PRINT(*sample); GTSAM_PRINT(sample);
} }
return 0; return 0;
} }

16
examples/DiscreteBayesNet_FG.cpp
View File

@ -33,11 +33,11 @@ using namespace gtsam;
int main(int argc, char **argv) { int main(int argc, char **argv) {
// Define keys and a print function // Define keys and a print function
Key C(1), S(2), R(3), W(4); Key C(1), S(2), R(3), W(4);
auto print = [=](DiscreteFactor::sharedValues values) { auto print = [=](const DiscreteFactor::Values& values) {
cout << boolalpha << "Cloudy = " << static_cast<bool>((*values)[C]) cout << boolalpha << "Cloudy = " << static_cast<bool>(values.at(C))
<< " Sprinkler = " << static_cast<bool>((*values)[S]) << " Sprinkler = " << static_cast<bool>(values.at(S))
<< " Rain = " << boolalpha << static_cast<bool>((*values)[R]) << " Rain = " << boolalpha << static_cast<bool>(values.at(R))
<< " WetGrass = " << static_cast<bool>((*values)[W]) << endl; << " WetGrass = " << static_cast<bool>(values.at(W)) << endl;
}; };
// We assume binary state variables // We assume binary state variables
@ -85,7 +85,7 @@ int main(int argc, char **argv) {
} }
// "Most Probable Explanation", i.e., configuration with largest value // "Most Probable Explanation", i.e., configuration with largest value
DiscreteFactor::sharedValues mpe = graph.eliminateSequential()->optimize(); auto mpe = graph.eliminateSequential()->optimize();
cout << "\nMost Probable Explanation (MPE):" << endl; cout << "\nMost Probable Explanation (MPE):" << endl;
print(mpe); print(mpe);
@ -97,7 +97,7 @@ int main(int argc, char **argv) {
// solve again, now with evidence // solve again, now with evidence
DiscreteBayesNet::shared_ptr chordal = graph.eliminateSequential(); DiscreteBayesNet::shared_ptr chordal = graph.eliminateSequential();
DiscreteFactor::sharedValues mpe_with_evidence = chordal->optimize(); auto mpe_with_evidence = chordal->optimize();
cout << "\nMPE given C=0:" << endl; cout << "\nMPE given C=0:" << endl;
print(mpe_with_evidence); print(mpe_with_evidence);
@ -113,7 +113,7 @@ int main(int argc, char **argv) {
// We can also sample from it // We can also sample from it
cout << "\n10 samples:" << endl; cout << "\n10 samples:" << endl;
for (size_t i = 0; i < 10; i++) { for (size_t i = 0; i < 10; i++) {
DiscreteFactor::sharedValues sample = chordal->sample(); auto sample = chordal->sample();
print(sample); print(sample);
} }
return 0; return 0;

8
examples/HMMExample.cpp
View File

@ -66,14 +66,14 @@ int main(int argc, char **argv) {
chordal->print("Eliminated"); chordal->print("Eliminated");
// solve // solve
DiscreteFactor::sharedValues mpe = chordal->optimize(); auto mpe = chordal->optimize();
GTSAM_PRINT(*mpe); GTSAM_PRINT(mpe);
// We can also sample from it // We can also sample from it
cout << "\n10 samples:" << endl; cout << "\n10 samples:" << endl;
for (size_t k = 0; k < 10; k++) { for (size_t k = 0; k < 10; k++) {
DiscreteFactor::sharedValues sample = chordal->sample(); auto sample = chordal->sample();
GTSAM_PRINT(*sample); GTSAM_PRINT(sample);
} }
// Or compute the marginals. This re-eliminates the FG into a Bayes tree // Or compute the marginals. This re-eliminates the FG into a Bayes tree

569
examples/IMUKittiExampleGPS.cpp
View File

@ -11,21 +11,23 @@
/** /**
* @file IMUKittiExampleGPS * @file IMUKittiExampleGPS
* @brief Example of application of ISAM2 for GPS-aided navigation on the KITTI VISION BENCHMARK SUITE * @brief Example of application of ISAM2 for GPS-aided navigation on the KITTI
* @author Ported by Thomas Jespersen (thomasj@tkjelectronics.dk), TKJ Electronics * VISION BENCHMARK SUITE
* @author Ported by Thomas Jespersen (thomasj@tkjelectronics.dk), TKJ
* Electronics
*/ */
// GTSAM related includes. // GTSAM related includes.
#include <gtsam/inference/Symbol.h>
#include <gtsam/navigation/CombinedImuFactor.h> #include <gtsam/navigation/CombinedImuFactor.h>
#include <gtsam/navigation/GPSFactor.h> #include <gtsam/navigation/GPSFactor.h>
#include <gtsam/navigation/ImuFactor.h> #include <gtsam/navigation/ImuFactor.h>
#include <gtsam/slam/dataset.h>
#include <gtsam/slam/BetweenFactor.h>
#include <gtsam/slam/PriorFactor.h>
#include <gtsam/nonlinear/ISAM2.h> #include <gtsam/nonlinear/ISAM2.h>
#include <gtsam/nonlinear/ISAM2Params.h> #include <gtsam/nonlinear/ISAM2Params.h>
#include <gtsam/nonlinear/NonlinearFactorGraph.h> #include <gtsam/nonlinear/NonlinearFactorGraph.h>
#include <gtsam/inference/Symbol.h> #include <gtsam/slam/BetweenFactor.h>
#include <gtsam/slam/PriorFactor.h>
#include <gtsam/slam/dataset.h>
#include <cstring> #include <cstring>
#include <fstream> #include <fstream>
@ -34,35 +36,35 @@
using namespace std; using namespace std;
using namespace gtsam; using namespace gtsam;
using symbol_shorthand::X; // Pose3 (x,y,z,r,p,y)
using symbol_shorthand::V; // Vel (xdot,ydot,zdot)
using symbol_shorthand::B; // Bias (ax,ay,az,gx,gy,gz) using symbol_shorthand::B; // Bias (ax,ay,az,gx,gy,gz)
using symbol_shorthand::V; // Vel (xdot,ydot,zdot)
using symbol_shorthand::X; // Pose3 (x,y,z,r,p,y)
struct KittiCalibration { struct KittiCalibration {
double body_ptx; double body_ptx;
double body_pty; double body_pty;
double body_ptz; double body_ptz;
double body_prx; double body_prx;
double body_pry; double body_pry;
double body_prz; double body_prz;
double accelerometer_sigma; double accelerometer_sigma;
double gyroscope_sigma; double gyroscope_sigma;
double integration_sigma; double integration_sigma;
double accelerometer_bias_sigma; double accelerometer_bias_sigma;
double gyroscope_bias_sigma; double gyroscope_bias_sigma;
double average_delta_t; double average_delta_t;
}; };
struct ImuMeasurement { struct ImuMeasurement {
double time; double time;
double dt; double dt;
Vector3 accelerometer; Vector3 accelerometer;
Vector3 gyroscope; // omega Vector3 gyroscope; // omega
}; };
struct GpsMeasurement { struct GpsMeasurement {
double time; double time;
Vector3 position; // x,y,z Vector3 position; // x,y,z
}; };
const string output_filename = "IMUKittiExampleGPSResults.csv"; const string output_filename = "IMUKittiExampleGPSResults.csv";
@ -70,290 +72,313 @@ const string output_filename = "IMUKittiExampleGPSResults.csv";
void loadKittiData(KittiCalibration& kitti_calibration, void loadKittiData(KittiCalibration& kitti_calibration,
vector<ImuMeasurement>& imu_measurements, vector<ImuMeasurement>& imu_measurements,
vector<GpsMeasurement>& gps_measurements) { vector<GpsMeasurement>& gps_measurements) {
string line; string line;
// Read IMU metadata and compute relative sensor pose transforms // Read IMU metadata and compute relative sensor pose transforms
// BodyPtx BodyPty BodyPtz BodyPrx BodyPry BodyPrz AccelerometerSigma GyroscopeSigma IntegrationSigma // BodyPtx BodyPty BodyPtz BodyPrx BodyPry BodyPrz AccelerometerSigma
// AccelerometerBiasSigma GyroscopeBiasSigma AverageDeltaT // GyroscopeSigma IntegrationSigma AccelerometerBiasSigma GyroscopeBiasSigma
string imu_metadata_file = findExampleDataFile("KittiEquivBiasedImu_metadata.txt"); // AverageDeltaT
ifstream imu_metadata(imu_metadata_file.c_str()); string imu_metadata_file =
findExampleDataFile("KittiEquivBiasedImu_metadata.txt");
ifstream imu_metadata(imu_metadata_file.c_str());
printf("-- Reading sensor metadata\n"); printf("-- Reading sensor metadata\n");
getline(imu_metadata, line, '\n'); // ignore the first line getline(imu_metadata, line, '\n'); // ignore the first line
// Load Kitti calibration // Load Kitti calibration
getline(imu_metadata, line, '\n'); getline(imu_metadata, line, '\n');
sscanf(line.c_str(), "%lf %lf %lf %lf %lf %lf %lf %lf %lf %lf %lf %lf", sscanf(line.c_str(), "%lf %lf %lf %lf %lf %lf %lf %lf %lf %lf %lf %lf",
&kitti_calibration.body_ptx, &kitti_calibration.body_ptx, &kitti_calibration.body_pty,
&kitti_calibration.body_pty, &kitti_calibration.body_ptz, &kitti_calibration.body_prx,
&kitti_calibration.body_ptz, &kitti_calibration.body_pry, &kitti_calibration.body_prz,
&kitti_calibration.body_prx, &kitti_calibration.accelerometer_sigma,
&kitti_calibration.body_pry, &kitti_calibration.gyroscope_sigma,
&kitti_calibration.body_prz, &kitti_calibration.integration_sigma,
&kitti_calibration.accelerometer_sigma, &kitti_calibration.accelerometer_bias_sigma,
&kitti_calibration.gyroscope_sigma, &kitti_calibration.gyroscope_bias_sigma,
&kitti_calibration.integration_sigma, &kitti_calibration.average_delta_t);
&kitti_calibration.accelerometer_bias_sigma, printf("IMU metadata: %lf %lf %lf %lf %lf %lf %lf %lf %lf %lf %lf %lf\n",
&kitti_calibration.gyroscope_bias_sigma, kitti_calibration.body_ptx, kitti_calibration.body_pty,
&kitti_calibration.average_delta_t); kitti_calibration.body_ptz, kitti_calibration.body_prx,
printf("IMU metadata: %lf %lf %lf %lf %lf %lf %lf %lf %lf %lf %lf %lf\n", kitti_calibration.body_pry, kitti_calibration.body_prz,
kitti_calibration.body_ptx, kitti_calibration.accelerometer_sigma,
kitti_calibration.body_pty, kitti_calibration.gyroscope_sigma, kitti_calibration.integration_sigma,
kitti_calibration.body_ptz, kitti_calibration.accelerometer_bias_sigma,
kitti_calibration.body_prx, kitti_calibration.gyroscope_bias_sigma,
kitti_calibration.body_pry, kitti_calibration.average_delta_t);
kitti_calibration.body_prz,
kitti_calibration.accelerometer_sigma,
kitti_calibration.gyroscope_sigma,
kitti_calibration.integration_sigma,
kitti_calibration.accelerometer_bias_sigma,
kitti_calibration.gyroscope_bias_sigma,
kitti_calibration.average_delta_t);
// Read IMU data // Read IMU data
// Time dt accelX accelY accelZ omegaX omegaY omegaZ // Time dt accelX accelY accelZ omegaX omegaY omegaZ
string imu_data_file = findExampleDataFile("KittiEquivBiasedImu.txt"); string imu_data_file = findExampleDataFile("KittiEquivBiasedImu.txt");
printf("-- Reading IMU measurements from file\n"); printf("-- Reading IMU measurements from file\n");
{ {
ifstream imu_data(imu_data_file.c_str()); ifstream imu_data(imu_data_file.c_str());
getline(imu_data, line, '\n'); // ignore the first line getline(imu_data, line, '\n'); // ignore the first line
double time = 0, dt = 0, acc_x = 0, acc_y = 0, acc_z = 0, gyro_x = 0, gyro_y = 0, gyro_z = 0; double time = 0, dt = 0, acc_x = 0, acc_y = 0, acc_z = 0, gyro_x = 0,
while (!imu_data.eof()) { gyro_y = 0, gyro_z = 0;
getline(imu_data, line, '\n'); while (!imu_data.eof()) {
sscanf(line.c_str(), "%lf %lf %lf %lf %lf %lf %lf %lf", getline(imu_data, line, '\n');
&time, &dt, sscanf(line.c_str(), "%lf %lf %lf %lf %lf %lf %lf %lf", &time, &dt,
&acc_x, &acc_y, &acc_z, &acc_x, &acc_y, &acc_z, &gyro_x, &gyro_y, &gyro_z);
&gyro_x, &gyro_y, &gyro_z);
ImuMeasurement measurement; ImuMeasurement measurement;
measurement.time = time; measurement.time = time;
measurement.dt = dt; measurement.dt = dt;
measurement.accelerometer = Vector3(acc_x, acc_y, acc_z); measurement.accelerometer = Vector3(acc_x, acc_y, acc_z);
measurement.gyroscope = Vector3(gyro_x, gyro_y, gyro_z); measurement.gyroscope = Vector3(gyro_x, gyro_y, gyro_z);
imu_measurements.push_back(measurement); imu_measurements.push_back(measurement);
}
} }
}
// Read GPS data // Read GPS data
// Time,X,Y,Z // Time,X,Y,Z
string gps_data_file = findExampleDataFile("KittiGps_converted.txt"); string gps_data_file = findExampleDataFile("KittiGps_converted.txt");
printf("-- Reading GPS measurements from file\n"); printf("-- Reading GPS measurements from file\n");
{ {
ifstream gps_data(gps_data_file.c_str()); ifstream gps_data(gps_data_file.c_str());
getline(gps_data, line, '\n'); // ignore the first line getline(gps_data, line, '\n'); // ignore the first line
double time = 0, gps_x = 0, gps_y = 0, gps_z = 0; double time = 0, gps_x = 0, gps_y = 0, gps_z = 0;
while (!gps_data.eof()) { while (!gps_data.eof()) {
getline(gps_data, line, '\n'); getline(gps_data, line, '\n');
sscanf(line.c_str(), "%lf,%lf,%lf,%lf", &time, &gps_x, &gps_y, &gps_z); sscanf(line.c_str(), "%lf,%lf,%lf,%lf", &time, &gps_x, &gps_y, &gps_z);
GpsMeasurement measurement; GpsMeasurement measurement;
measurement.time = time; measurement.time = time;
measurement.position = Vector3(gps_x, gps_y, gps_z); measurement.position = Vector3(gps_x, gps_y, gps_z);
gps_measurements.push_back(measurement); gps_measurements.push_back(measurement);
}
} }
}
} }
int main(int argc, char* argv[]) { int main(int argc, char* argv[]) {
KittiCalibration kitti_calibration; KittiCalibration kitti_calibration;
vector<ImuMeasurement> imu_measurements; vector<ImuMeasurement> imu_measurements;
vector<GpsMeasurement> gps_measurements; vector<GpsMeasurement> gps_measurements;
loadKittiData(kitti_calibration, imu_measurements, gps_measurements); loadKittiData(kitti_calibration, imu_measurements, gps_measurements);
Vector6 BodyP = (Vector6() << kitti_calibration.body_ptx, kitti_calibration.body_pty, kitti_calibration.body_ptz, Vector6 BodyP =
kitti_calibration.body_prx, kitti_calibration.body_pry, kitti_calibration.body_prz) (Vector6() << kitti_calibration.body_ptx, kitti_calibration.body_pty,
.finished(); kitti_calibration.body_ptz, kitti_calibration.body_prx,
auto body_T_imu = Pose3::Expmap(BodyP); kitti_calibration.body_pry, kitti_calibration.body_prz)
if (!body_T_imu.equals(Pose3(), 1e-5)) { .finished();
printf("Currently only support IMUinBody is identity, i.e. IMU and body frame are the same"); auto body_T_imu = Pose3::Expmap(BodyP);
exit(-1); if (!body_T_imu.equals(Pose3(), 1e-5)) {
} printf(
"Currently only support IMUinBody is identity, i.e. IMU and body frame "
"are the same");
exit(-1);
}
// Configure different variables // Configure different variables
// double t_offset = gps_measurements[0].time; // double t_offset = gps_measurements[0].time;
size_t first_gps_pose = 1; size_t first_gps_pose = 1;
size_t gps_skip = 10; // Skip this many GPS measurements each time size_t gps_skip = 10; // Skip this many GPS measurements each time
double g = 9.8; double g = 9.8;
auto w_coriolis = Vector3::Zero(); // zero vector auto w_coriolis = Vector3::Zero(); // zero vector
// Configure noise models // Configure noise models
auto noise_model_gps = noiseModel::Diagonal::Precisions((Vector6() << Vector3::Constant(0), auto noise_model_gps = noiseModel::Diagonal::Precisions(
Vector3::Constant(1.0/0.07)) (Vector6() << Vector3::Constant(0), Vector3::Constant(1.0 / 0.07))
.finished()); .finished());
// Set initial conditions for the estimated trajectory // Set initial conditions for the estimated trajectory
// initial pose is the reference frame (navigation frame) // initial pose is the reference frame (navigation frame)
auto current_pose_global = Pose3(Rot3(), gps_measurements[first_gps_pose].position); auto current_pose_global =
// the vehicle is stationary at the beginning at position 0,0,0 Pose3(Rot3(), gps_measurements[first_gps_pose].position);
Vector3 current_velocity_global = Vector3::Zero(); // the vehicle is stationary at the beginning at position 0,0,0
auto current_bias = imuBias::ConstantBias(); // init with zero bias Vector3 current_velocity_global = Vector3::Zero();
auto current_bias = imuBias::ConstantBias(); // init with zero bias
auto sigma_init_x = noiseModel::Diagonal::Precisions((Vector6() << Vector3::Constant(0), auto sigma_init_x = noiseModel::Diagonal::Precisions(
Vector3::Constant(1.0)) (Vector6() << Vector3::Constant(0), Vector3::Constant(1.0)).finished());
.finished()); auto sigma_init_v = noiseModel::Diagonal::Sigmas(Vector3::Constant(1000.0));
auto sigma_init_v = noiseModel::Diagonal::Sigmas(Vector3::Constant(1000.0)); auto sigma_init_b = noiseModel::Diagonal::Sigmas(
auto sigma_init_b = noiseModel::Diagonal::Sigmas((Vector6() << Vector3::Constant(0.100), (Vector6() << Vector3::Constant(0.100), Vector3::Constant(5.00e-05))
Vector3::Constant(5.00e-05)) .finished());
.finished());
// Set IMU preintegration parameters // Set IMU preintegration parameters
Matrix33 measured_acc_cov = I_3x3 * pow(kitti_calibration.accelerometer_sigma, 2); Matrix33 measured_acc_cov =
Matrix33 measured_omega_cov = I_3x3 * pow(kitti_calibration.gyroscope_sigma, 2); I_3x3 * pow(kitti_calibration.accelerometer_sigma, 2);
// error committed in integrating position from velocities Matrix33 measured_omega_cov =
Matrix33 integration_error_cov = I_3x3 * pow(kitti_calibration.integration_sigma, 2); I_3x3 * pow(kitti_calibration.gyroscope_sigma, 2);
// error committed in integrating position from velocities
Matrix33 integration_error_cov =
I_3x3 * pow(kitti_calibration.integration_sigma, 2);
auto imu_params = PreintegratedImuMeasurements::Params::MakeSharedU(g); auto imu_params = PreintegratedImuMeasurements::Params::MakeSharedU(g);
imu_params->accelerometerCovariance = measured_acc_cov; // acc white noise in continuous imu_params->accelerometerCovariance =
imu_params->integrationCovariance = integration_error_cov; // integration uncertainty continuous measured_acc_cov; // acc white noise in continuous
imu_params->gyroscopeCovariance = measured_omega_cov; // gyro white noise in continuous imu_params->integrationCovariance =
imu_params->omegaCoriolis = w_coriolis; integration_error_cov; // integration uncertainty continuous
imu_params->gyroscopeCovariance =
measured_omega_cov; // gyro white noise in continuous
imu_params->omegaCoriolis = w_coriolis;
std::shared_ptr<PreintegratedImuMeasurements> current_summarized_measurement = nullptr; std::shared_ptr<PreintegratedImuMeasurements> current_summarized_measurement =
nullptr;
// Set ISAM2 parameters and create ISAM2 solver object // Set ISAM2 parameters and create ISAM2 solver object
ISAM2Params isam_params; ISAM2Params isam_params;
isam_params.factorization = ISAM2Params::CHOLESKY; isam_params.factorization = ISAM2Params::CHOLESKY;
isam_params.relinearizeSkip = 10; isam_params.relinearizeSkip = 10;
ISAM2 isam(isam_params); ISAM2 isam(isam_params);
// Create the factor graph and values object that will store new factors and values to add to the incremental graph // Create the factor graph and values object that will store new factors and
NonlinearFactorGraph new_factors; // values to add to the incremental graph
Values new_values; // values storing the initial estimates of new nodes in the factor graph NonlinearFactorGraph new_factors;
Values new_values; // values storing the initial estimates of new nodes in
// the factor graph
/// Main loop: /// Main loop:
/// (1) we read the measurements /// (1) we read the measurements
/// (2) we create the corresponding factors in the graph /// (2) we create the corresponding factors in the graph
/// (3) we solve the graph to obtain and optimal estimate of robot trajectory /// (3) we solve the graph to obtain and optimal estimate of robot trajectory
printf("-- Starting main loop: inference is performed at each time step, but we plot trajectory every 10 steps\n"); printf(
size_t j = 0; "-- Starting main loop: inference is performed at each time step, but we "
for (size_t i = first_gps_pose; i < gps_measurements.size() - 1; i++) { "plot trajectory every 10 steps\n");
// At each non=IMU measurement we initialize a new node in the graph size_t j = 0;
auto current_pose_key = X(i); size_t included_imu_measurement_count = 0;
auto current_vel_key = V(i);
auto current_bias_key = B(i);
double t = gps_measurements[i].time;
if (i == first_gps_pose) { for (size_t i = first_gps_pose; i < gps_measurements.size() - 1; i++) {
// Create initial estimate and prior on initial pose, velocity, and biases // At each non=IMU measurement we initialize a new node in the graph
new_values.insert(current_pose_key, current_pose_global); auto current_pose_key = X(i);
new_values.insert(current_vel_key, current_velocity_global); auto current_vel_key = V(i);
new_values.insert(current_bias_key, current_bias); auto current_bias_key = B(i);
new_factors.emplace_shared<PriorFactor<Pose3>>(current_pose_key, current_pose_global, sigma_init_x); double t = gps_measurements[i].time;
new_factors.emplace_shared<PriorFactor<Vector3>>(current_vel_key, current_velocity_global, sigma_init_v);
new_factors.emplace_shared<PriorFactor<imuBias::ConstantBias>>(current_bias_key, current_bias, sigma_init_b);
} else {
double t_previous = gps_measurements[i-1].time;
// Summarize IMU data between the previous GPS measurement and now if (i == first_gps_pose) {
current_summarized_measurement = std::make_shared<PreintegratedImuMeasurements>(imu_params, current_bias); // Create initial estimate and prior on initial pose, velocity, and biases
static size_t included_imu_measurement_count = 0; new_values.insert(current_pose_key, current_pose_global);
while (j < imu_measurements.size() && imu_measurements[j].time <= t) { new_values.insert(current_vel_key, current_velocity_global);
if (imu_measurements[j].time >= t_previous) { new_values.insert(current_bias_key, current_bias);
current_summarized_measurement->integrateMeasurement(imu_measurements[j].accelerometer, new_factors.emplace_shared<PriorFactor<Pose3>>(
imu_measurements[j].gyroscope, current_pose_key, current_pose_global, sigma_init_x);
imu_measurements[j].dt); new_factors.emplace_shared<PriorFactor<Vector3>>(
included_imu_measurement_count++; current_vel_key, current_velocity_global, sigma_init_v);
} new_factors.emplace_shared<PriorFactor<imuBias::ConstantBias>>(
j++; current_bias_key, current_bias, sigma_init_b);
} } else {
double t_previous = gps_measurements[i - 1].time;
// Create IMU factor // Summarize IMU data between the previous GPS measurement and now
auto previous_pose_key = X(i-1); current_summarized_measurement =
auto previous_vel_key = V(i-1); std::make_shared<PreintegratedImuMeasurements>(imu_params,
auto previous_bias_key = B(i-1); current_bias);
new_factors.emplace_shared<ImuFactor>(previous_pose_key, previous_vel_key, while (j < imu_measurements.size() && imu_measurements[j].time <= t) {
current_pose_key, current_vel_key, if (imu_measurements[j].time >= t_previous) {
previous_bias_key, *current_summarized_measurement); current_summarized_measurement->integrateMeasurement(
imu_measurements[j].accelerometer, imu_measurements[j].gyroscope,
// Bias evolution as given in the IMU metadata imu_measurements[j].dt);
auto sigma_between_b = noiseModel::Diagonal::Sigmas((Vector6() << included_imu_measurement_count++;
Vector3::Constant(sqrt(included_imu_measurement_count) * kitti_calibration.accelerometer_bias_sigma),
Vector3::Constant(sqrt(included_imu_measurement_count) * kitti_calibration.gyroscope_bias_sigma))
.finished());
new_factors.emplace_shared<BetweenFactor<imuBias::ConstantBias>>(previous_bias_key,
current_bias_key,
imuBias::ConstantBias(),
sigma_between_b);
// Create GPS factor
auto gps_pose = Pose3(current_pose_global.rotation(), gps_measurements[i].position);
if ((i % gps_skip) == 0) {
new_factors.emplace_shared<PriorFactor<Pose3>>(current_pose_key, gps_pose, noise_model_gps);
new_values.insert(current_pose_key, gps_pose);
printf("################ POSE INCLUDED AT TIME %lf ################\n", t);
cout << gps_pose.translation();
printf("\n\n");
} else {
new_values.insert(current_pose_key, current_pose_global);
}
// Add initial values for velocity and bias based on the previous estimates
new_values.insert(current_vel_key, current_velocity_global);
new_values.insert(current_bias_key, current_bias);
// Update solver
// =======================================================================
// We accumulate 2*GPSskip GPS measurements before updating the solver at
// first so that the heading becomes observable.
if (i > (first_gps_pose + 2*gps_skip)) {
printf("################ NEW FACTORS AT TIME %lf ################\n", t);
new_factors.print();
isam.update(new_factors, new_values);
// Reset the newFactors and newValues list
new_factors.resize(0);
new_values.clear();
// Extract the result/current estimates
Values result = isam.calculateEstimate();
current_pose_global = result.at<Pose3>(current_pose_key);
current_velocity_global = result.at<Vector3>(current_vel_key);
current_bias = result.at<imuBias::ConstantBias>(current_bias_key);
printf("\n################ POSE AT TIME %lf ################\n", t);
current_pose_global.print();
printf("\n\n");
}
} }
j++;
}
// Create IMU factor
auto previous_pose_key = X(i - 1);
auto previous_vel_key = V(i - 1);
auto previous_bias_key = B(i - 1);
new_factors.emplace_shared<ImuFactor>(
previous_pose_key, previous_vel_key, current_pose_key,
current_vel_key, previous_bias_key, *current_summarized_measurement);
// Bias evolution as given in the IMU metadata
auto sigma_between_b = noiseModel::Diagonal::Sigmas(
(Vector6() << Vector3::Constant(
sqrt(included_imu_measurement_count) *
kitti_calibration.accelerometer_bias_sigma),
Vector3::Constant(sqrt(included_imu_measurement_count) *
kitti_calibration.gyroscope_bias_sigma))
.finished());
new_factors.emplace_shared<BetweenFactor<imuBias::ConstantBias>>(
previous_bias_key, current_bias_key, imuBias::ConstantBias(),
sigma_between_b);
// Create GPS factor
auto gps_pose =
Pose3(current_pose_global.rotation(), gps_measurements[i].position);
if ((i % gps_skip) == 0) {
new_factors.emplace_shared<PriorFactor<Pose3>>(
current_pose_key, gps_pose, noise_model_gps);
new_values.insert(current_pose_key, gps_pose);
printf("############ POSE INCLUDED AT TIME %.6lf ############\n",
t);
cout << gps_pose.translation();
printf("\n\n");
} else {
new_values.insert(current_pose_key, current_pose_global);
}
// Add initial values for velocity and bias based on the previous
// estimates
new_values.insert(current_vel_key, current_velocity_global);
new_values.insert(current_bias_key, current_bias);
// Update solver
// =======================================================================
// We accumulate 2*GPSskip GPS measurements before updating the solver at
// first so that the heading becomes observable.
if (i > (first_gps_pose + 2 * gps_skip)) {
printf("############ NEW FACTORS AT TIME %.6lf ############\n",
t);
new_factors.print();
isam.update(new_factors, new_values);
// Reset the newFactors and newValues list
new_factors.resize(0);
new_values.clear();
// Extract the result/current estimates
Values result = isam.calculateEstimate();
current_pose_global = result.at<Pose3>(current_pose_key);
current_velocity_global = result.at<Vector3>(current_vel_key);
current_bias = result.at<imuBias::ConstantBias>(current_bias_key);
printf("\n############ POSE AT TIME %lf ############\n", t);
current_pose_global.print();
printf("\n\n");
}
} }
}
// Save results to file // Save results to file
printf("\nWriting results to file...\n"); printf("\nWriting results to file...\n");
FILE* fp_out = fopen(output_filename.c_str(), "w+"); FILE* fp_out = fopen(output_filename.c_str(), "w+");
fprintf(fp_out, "#time(s),x(m),y(m),z(m),qx,qy,qz,qw,gt_x(m),gt_y(m),gt_z(m)\n"); fprintf(fp_out,
"#time(s),x(m),y(m),z(m),qx,qy,qz,qw,gt_x(m),gt_y(m),gt_z(m)\n");
Values result = isam.calculateEstimate(); Values result = isam.calculateEstimate();
for (size_t i = first_gps_pose; i < gps_measurements.size() - 1; i++) { for (size_t i = first_gps_pose; i < gps_measurements.size() - 1; i++) {
auto pose_key = X(i); auto pose_key = X(i);
auto vel_key = V(i); auto vel_key = V(i);
auto bias_key = B(i); auto bias_key = B(i);
auto pose = result.at<Pose3>(pose_key); auto pose = result.at<Pose3>(pose_key);
auto velocity = result.at<Vector3>(vel_key); auto velocity = result.at<Vector3>(vel_key);
auto bias = result.at<imuBias::ConstantBias>(bias_key); auto bias = result.at<imuBias::ConstantBias>(bias_key);
auto pose_quat = pose.rotation().toQuaternion(); auto pose_quat = pose.rotation().toQuaternion();
auto gps = gps_measurements[i].position; auto gps = gps_measurements[i].position;
cout << "State at #" << i << endl; cout << "State at #" << i << endl;
cout << "Pose:" << endl << pose << endl; cout << "Pose:" << endl << pose << endl;
cout << "Velocity:" << endl << velocity << endl; cout << "Velocity:" << endl << velocity << endl;
cout << "Bias:" << endl << bias << endl; cout << "Bias:" << endl << bias << endl;
fprintf(fp_out, "%f,%f,%f,%f,%f,%f,%f,%f,%f,%f,%f\n", fprintf(fp_out, "%f,%f,%f,%f,%f,%f,%f,%f,%f,%f,%f\n",
gps_measurements[i].time, gps_measurements[i].time, pose.x(), pose.y(), pose.z(),
pose.x(), pose.y(), pose.z(), pose_quat.x(), pose_quat.y(), pose_quat.z(), pose_quat.w(), gps(0),
pose_quat.x(), pose_quat.y(), pose_quat.z(), pose_quat.w(), gps(1), gps(2));
gps(0), gps(1), gps(2)); }
}
fclose(fp_out); fclose(fp_out);
} }

4
examples/UGM_chain.cpp
View File

@ -70,8 +70,8 @@ int main(int argc, char** argv) {
// "Decoding", i.e., configuration with largest value // "Decoding", i.e., configuration with largest value
// We use sequential variable elimination // We use sequential variable elimination
DiscreteBayesNet::shared_ptr chordal = graph.eliminateSequential(); DiscreteBayesNet::shared_ptr chordal = graph.eliminateSequential();
DiscreteFactor::sharedValues optimalDecoding = chordal->optimize(); auto optimalDecoding = chordal->optimize();
optimalDecoding->print("\nMost Probable Explanation (optimalDecoding)\n"); optimalDecoding.print("\nMost Probable Explanation (optimalDecoding)\n");
// "Inference" Computing marginals for each node // "Inference" Computing marginals for each node
// Here we'll make use of DiscreteMarginals class, which makes use of // Here we'll make use of DiscreteMarginals class, which makes use of

4
examples/UGM_small.cpp
View File

@ -63,8 +63,8 @@ int main(int argc, char** argv) {
// "Decoding", i.e., configuration with largest value (MPE) // "Decoding", i.e., configuration with largest value (MPE)
// We use sequential variable elimination // We use sequential variable elimination
DiscreteBayesNet::shared_ptr chordal = graph.eliminateSequential(); DiscreteBayesNet::shared_ptr chordal = graph.eliminateSequential();
DiscreteFactor::sharedValues optimalDecoding = chordal->optimize(); auto optimalDecoding = chordal->optimize();
optimalDecoding->print("\noptimalDecoding"); GTSAM_PRINT(optimalDecoding);
// "Inference" Computing marginals // "Inference" Computing marginals
cout << "\nComputing Node Marginals .." << endl; cout << "\nComputing Node Marginals .." << endl;

2
gtsam/base/Lie.h
View File

@ -370,4 +370,4 @@ public:
* the gtsam namespace to be more easily enforced as testable * the gtsam namespace to be more easily enforced as testable
*/ */
#define GTSAM_CONCEPT_LIE_INST(T) template class gtsam::IsLieGroup<T>; #define GTSAM_CONCEPT_LIE_INST(T) template class gtsam::IsLieGroup<T>;
#define GTSAM_CONCEPT_LIE_TYPE(T) typedef gtsam::IsLieGroup<T> _gtsam_IsLieGroup_##T; #define GTSAM_CONCEPT_LIE_TYPE(T) using _gtsam_IsLieGroup_##T = gtsam::IsLieGroup<T>;

2
gtsam/base/Manifold.h
View File

@ -178,4 +178,4 @@ struct FixedDimension {
// * the gtsam namespace to be more easily enforced as testable // * the gtsam namespace to be more easily enforced as testable
// */ // */
#define GTSAM_CONCEPT_MANIFOLD_INST(T) template class gtsam::IsManifold<T>; #define GTSAM_CONCEPT_MANIFOLD_INST(T) template class gtsam::IsManifold<T>;
#define GTSAM_CONCEPT_MANIFOLD_TYPE(T) typedef gtsam::IsManifold<T> _gtsam_IsManifold_##T; #define GTSAM_CONCEPT_MANIFOLD_TYPE(T) using _gtsam_IsManifold_##T = gtsam::IsManifold<T>;

38
gtsam/base/Matrix.h
View File

@ -46,28 +46,28 @@ typedef Eigen::Matrix<double, Eigen::Dynamic, Eigen::Dynamic, Eigen::RowMajor> M
// Create handy typedefs and constants for square-size matrices // Create handy typedefs and constants for square-size matrices
// MatrixMN, MatrixN = MatrixNN, I_NxN, and Z_NxN, for M,N=1..9 // MatrixMN, MatrixN = MatrixNN, I_NxN, and Z_NxN, for M,N=1..9
#define GTSAM_MAKE_MATRIX_DEFS(N) \ #define GTSAM_MAKE_MATRIX_DEFS(N) \
typedef Eigen::Matrix<double, N, N> Matrix##N; \ using Matrix##N = Eigen::Matrix<double, N, N>; \
typedef Eigen::Matrix<double, 1, N> Matrix1##N; \ using Matrix1##N = Eigen::Matrix<double, 1, N>; \
typedef Eigen::Matrix<double, 2, N> Matrix2##N; \ using Matrix2##N = Eigen::Matrix<double, 2, N>; \
typedef Eigen::Matrix<double, 3, N> Matrix3##N; \ using Matrix3##N = Eigen::Matrix<double, 3, N>; \
typedef Eigen::Matrix<double, 4, N> Matrix4##N; \ using Matrix4##N = Eigen::Matrix<double, 4, N>; \
typedef Eigen::Matrix<double, 5, N> Matrix5##N; \ using Matrix5##N = Eigen::Matrix<double, 5, N>; \
typedef Eigen::Matrix<double, 6, N> Matrix6##N; \ using Matrix6##N = Eigen::Matrix<double, 6, N>; \
typedef Eigen::Matrix<double, 7, N> Matrix7##N; \ using Matrix7##N = Eigen::Matrix<double, 7, N>; \
typedef Eigen::Matrix<double, 8, N> Matrix8##N; \ using Matrix8##N = Eigen::Matrix<double, 8, N>; \
typedef Eigen::Matrix<double, 9, N> Matrix9##N; \ using Matrix9##N = Eigen::Matrix<double, 9, N>; \
static const Eigen::MatrixBase<Matrix##N>::IdentityReturnType I_##N##x##N = Matrix##N::Identity(); \ static const Eigen::MatrixBase<Matrix##N>::IdentityReturnType I_##N##x##N = Matrix##N::Identity(); \
static const Eigen::MatrixBase<Matrix##N>::ConstantReturnType Z_##N##x##N = Matrix##N::Zero(); static const Eigen::MatrixBase<Matrix##N>::ConstantReturnType Z_##N##x##N = Matrix##N::Zero();
GTSAM_MAKE_MATRIX_DEFS(1); GTSAM_MAKE_MATRIX_DEFS(1)
GTSAM_MAKE_MATRIX_DEFS(2); GTSAM_MAKE_MATRIX_DEFS(2)
GTSAM_MAKE_MATRIX_DEFS(3); GTSAM_MAKE_MATRIX_DEFS(3)
GTSAM_MAKE_MATRIX_DEFS(4); GTSAM_MAKE_MATRIX_DEFS(4)
GTSAM_MAKE_MATRIX_DEFS(5); GTSAM_MAKE_MATRIX_DEFS(5)
GTSAM_MAKE_MATRIX_DEFS(6); GTSAM_MAKE_MATRIX_DEFS(6)
GTSAM_MAKE_MATRIX_DEFS(7); GTSAM_MAKE_MATRIX_DEFS(7)
GTSAM_MAKE_MATRIX_DEFS(8); GTSAM_MAKE_MATRIX_DEFS(8)
GTSAM_MAKE_MATRIX_DEFS(9); GTSAM_MAKE_MATRIX_DEFS(9)
// Matrix expressions for accessing parts of matrices // Matrix expressions for accessing parts of matrices
typedef Eigen::Block<Matrix> SubMatrix; typedef Eigen::Block<Matrix> SubMatrix;

2
gtsam/base/Testable.h
View File

@ -173,4 +173,4 @@ namespace gtsam {
* @deprecated please use BOOST_CONCEPT_ASSERT and * @deprecated please use BOOST_CONCEPT_ASSERT and
*/ */
#define GTSAM_CONCEPT_TESTABLE_INST(T) template class gtsam::IsTestable<T>; #define GTSAM_CONCEPT_TESTABLE_INST(T) template class gtsam::IsTestable<T>;
#define GTSAM_CONCEPT_TESTABLE_TYPE(T) typedef gtsam::IsTestable<T> _gtsam_Testable_##T; #define GTSAM_CONCEPT_TESTABLE_TYPE(T) using _gtsam_Testable_##T = gtsam::IsTestable<T>;

2
gtsam/base/TestableAssertions.h
View File

@ -85,7 +85,7 @@ bool assert_equal(const V& expected, const boost::optional<const V&>& actual, do
* \deprecated: use container equals instead * \deprecated: use container equals instead
*/ */
template<class V> template<class V>
bool assert_equal(const std::vector<V>& expected, const std::vector<V>& actual, double tol = 1e-9) { bool GTSAM_DEPRECATED assert_equal(const std::vector<V>& expected, const std::vector<V>& actual, double tol = 1e-9) {
bool match = true; bool match = true;
if (expected.size() != actual.size()) if (expected.size() != actual.size())
match = false; match = false;

24
gtsam/base/Vector.h
View File

@ -48,18 +48,18 @@ static const Eigen::MatrixBase<Vector3>::ConstantReturnType Z_3x1 = Vector3::Zer
// Create handy typedefs and constants for vectors with N>3 // Create handy typedefs and constants for vectors with N>3
// VectorN and Z_Nx1, for N=1..9 // VectorN and Z_Nx1, for N=1..9
#define GTSAM_MAKE_VECTOR_DEFS(N) \ #define GTSAM_MAKE_VECTOR_DEFS(N) \
typedef Eigen::Matrix<double, N, 1> Vector##N; \ using Vector##N = Eigen::Matrix<double, N, 1>; \
static const Eigen::MatrixBase<Vector##N>::ConstantReturnType Z_##N##x1 = Vector##N::Zero(); static const Eigen::MatrixBase<Vector##N>::ConstantReturnType Z_##N##x1 = Vector##N::Zero();
GTSAM_MAKE_VECTOR_DEFS(4); GTSAM_MAKE_VECTOR_DEFS(4)
GTSAM_MAKE_VECTOR_DEFS(5); GTSAM_MAKE_VECTOR_DEFS(5)
GTSAM_MAKE_VECTOR_DEFS(6); GTSAM_MAKE_VECTOR_DEFS(6)
GTSAM_MAKE_VECTOR_DEFS(7); GTSAM_MAKE_VECTOR_DEFS(7)
GTSAM_MAKE_VECTOR_DEFS(8); GTSAM_MAKE_VECTOR_DEFS(8)
GTSAM_MAKE_VECTOR_DEFS(9); GTSAM_MAKE_VECTOR_DEFS(9)
GTSAM_MAKE_VECTOR_DEFS(10); GTSAM_MAKE_VECTOR_DEFS(10)
GTSAM_MAKE_VECTOR_DEFS(11); GTSAM_MAKE_VECTOR_DEFS(11)
GTSAM_MAKE_VECTOR_DEFS(12); GTSAM_MAKE_VECTOR_DEFS(12)
typedef Eigen::VectorBlock<Vector> SubVector; typedef Eigen::VectorBlock<Vector> SubVector;
typedef Eigen::VectorBlock<const Vector> ConstSubVector; typedef Eigen::VectorBlock<const Vector> ConstSubVector;
@ -207,14 +207,14 @@ inline double inner_prod(const V1 &a, const V2& b) {
* BLAS Level 1 scal: x <- alpha*x * BLAS Level 1 scal: x <- alpha*x
* \deprecated: use operators instead * \deprecated: use operators instead
*/ */
inline void scal(double alpha, Vector& x) { x *= alpha; } inline void GTSAM_DEPRECATED scal(double alpha, Vector& x) { x *= alpha; }
/** /**
* BLAS Level 1 axpy: y <- alpha*x + y * BLAS Level 1 axpy: y <- alpha*x + y
* \deprecated: use operators instead * \deprecated: use operators instead
*/ */
template<class V1, class V2> template<class V1, class V2>
inline void axpy(double alpha, const V1& x, V2& y) { inline void GTSAM_DEPRECATED axpy(double alpha, const V1& x, V2& y) {
assert (y.size()==x.size()); assert (y.size()==x.size());
y += alpha * x; y += alpha * x;
} }

2
gtsam/base/chartTesting.h
View File

@ -32,7 +32,7 @@ void testDefaultChart(TestResult& result_,
const std::string& name_, const std::string& name_,
const T& value) { const T& value) {
GTSAM_CONCEPT_TESTABLE_TYPE(T); GTSAM_CONCEPT_TESTABLE_TYPE(T)
typedef typename gtsam::DefaultChart<T> Chart; typedef typename gtsam::DefaultChart<T> Chart;
typedef typename Chart::vector Vector; typedef typename Chart::vector Vector;

14
gtsam/base/serialization.h
View File

@ -19,8 +19,9 @@
#pragma once #pragma once
#include <sstream> #include <Eigen/Core>
#include <fstream> #include <fstream>
#include <sstream>
#include <string> #include <string>
// includes for standard serialization types // includes for standard serialization types
@ -40,6 +41,17 @@
#include <boost/archive/binary_oarchive.hpp> #include <boost/archive/binary_oarchive.hpp>
#include <boost/serialization/export.hpp> #include <boost/serialization/export.hpp>
// Workaround a bug in GCC >= 7 and C++17
// ref. https://gitlab.com/libeigen/eigen/-/issues/1676
#ifdef __GNUC__
#if __GNUC__ >= 7 && __cplusplus >= 201703L
namespace boost { namespace serialization { struct U; } }
namespace Eigen { namespace internal {
template<> struct traits<boost::serialization::U> {enum {Flags=0};};
} }
#endif
#endif
namespace gtsam { namespace gtsam {
/** @name Standard serialization /** @name Standard serialization

4
gtsam/base/tests/testVector.cpp
View File

@ -220,8 +220,8 @@ TEST(Vector, axpy )
Vector x = Vector3(10., 20., 30.); Vector x = Vector3(10., 20., 30.);
Vector y0 = Vector3(2.0, 5.0, 6.0); Vector y0 = Vector3(2.0, 5.0, 6.0);
Vector y1 = y0, y2 = y0; Vector y1 = y0, y2 = y0;
axpy(0.1,x,y1); y1 += 0.1 * x;
axpy(0.1,x,y2.head(3)); y2.head(3) += 0.1 * x;
Vector expected = Vector3(3.0, 7.0, 9.0); Vector expected = Vector3(3.0, 7.0, 9.0);
EXPECT(assert_equal(expected,y1)); EXPECT(assert_equal(expected,y1));
EXPECT(assert_equal(expected,Vector(y2))); EXPECT(assert_equal(expected,Vector(y2)));

5
gtsam/base/treeTraversal-inst.h
View File

@ -158,9 +158,8 @@ void DepthFirstForestParallel(FOREST& forest, DATA& rootData,
// Typedefs // Typedefs
typedef typename FOREST::Node Node; typedef typename FOREST::Node Node;
tbb::task::spawn_root_and_wait( internal::CreateRootTask<Node>(forest.roots(), rootData, visitorPre,
internal::CreateRootTask<Node>(forest.roots(), rootData, visitorPre, visitorPost, problemSizeThreshold);
visitorPost, problemSizeThreshold));
#else #else
DepthFirstForest(forest, rootData, visitorPre, visitorPost); DepthFirstForest(forest, rootData, visitorPre, visitorPost);
#endif #endif

70
gtsam/base/treeTraversal/parallelTraversalTasks.h
View File

@ -22,7 +22,7 @@
#include <boost/make_shared.hpp> #include <boost/make_shared.hpp>
#ifdef GTSAM_USE_TBB #ifdef GTSAM_USE_TBB
#include <tbb/task.h> // tbb::task, tbb::task_list #include <tbb/task_group.h> // tbb::task_group
#include <tbb/scalable_allocator.h> // tbb::scalable_allocator #include <tbb/scalable_allocator.h> // tbb::scalable_allocator
namespace gtsam { namespace gtsam {
@ -34,7 +34,7 @@ namespace gtsam {
/* ************************************************************************* */ /* ************************************************************************* */
template<typename NODE, typename DATA, typename VISITOR_PRE, typename VISITOR_POST> template<typename NODE, typename DATA, typename VISITOR_PRE, typename VISITOR_POST>
class PreOrderTask : public tbb::task class PreOrderTask
{ {
public: public:
const boost::shared_ptr<NODE>& treeNode; const boost::shared_ptr<NODE>& treeNode;
@ -42,28 +42,30 @@ namespace gtsam {
VISITOR_PRE& visitorPre; VISITOR_PRE& visitorPre;
VISITOR_POST& visitorPost; VISITOR_POST& visitorPost;
int problemSizeThreshold; int problemSizeThreshold;
tbb::task_group& tg;
bool makeNewTasks; bool makeNewTasks;
bool isPostOrderPhase; // Keep track of order phase across multiple calls to the same functor
mutable bool isPostOrderPhase;
PreOrderTask(const boost::shared_ptr<NODE>& treeNode, const boost::shared_ptr<DATA>& myData, PreOrderTask(const boost::shared_ptr<NODE>& treeNode, const boost::shared_ptr<DATA>& myData,
VISITOR_PRE& visitorPre, VISITOR_POST& visitorPost, int problemSizeThreshold, VISITOR_PRE& visitorPre, VISITOR_POST& visitorPost, int problemSizeThreshold,
bool makeNewTasks = true) tbb::task_group& tg, bool makeNewTasks = true)
: treeNode(treeNode), : treeNode(treeNode),
myData(myData), myData(myData),
visitorPre(visitorPre), visitorPre(visitorPre),
visitorPost(visitorPost), visitorPost(visitorPost),
problemSizeThreshold(problemSizeThreshold), problemSizeThreshold(problemSizeThreshold),
tg(tg),
makeNewTasks(makeNewTasks), makeNewTasks(makeNewTasks),
isPostOrderPhase(false) {} isPostOrderPhase(false) {}
tbb::task* execute() override void operator()() const
{ {
if(isPostOrderPhase) if(isPostOrderPhase)
{ {
// Run the post-order visitor since this task was recycled to run the post-order visitor // Run the post-order visitor since this task was recycled to run the post-order visitor
(void) visitorPost(treeNode, *myData); (void) visitorPost(treeNode, *myData);
return nullptr;
} }
else else
{ {
@ -71,14 +73,10 @@ namespace gtsam {
{ {
if(!treeNode->children.empty()) if(!treeNode->children.empty())
{ {
// Allocate post-order task as a continuation
isPostOrderPhase = true;
recycle_as_continuation();
bool overThreshold = (treeNode->problemSize() >= problemSizeThreshold); bool overThreshold = (treeNode->problemSize() >= problemSizeThreshold);
tbb::task* firstChild = 0; // If we have child tasks, start subtasks and wait for them to complete
tbb::task_list childTasks; tbb::task_group ctg;
for(const boost::shared_ptr<NODE>& child: treeNode->children) for(const boost::shared_ptr<NODE>& child: treeNode->children)
{ {
// Process child in a subtask. Important: Run visitorPre before calling // Process child in a subtask. Important: Run visitorPre before calling
@ -86,37 +84,30 @@ namespace gtsam {
// allocated an extra child, this causes a TBB error. // allocated an extra child, this causes a TBB error.
boost::shared_ptr<DATA> childData = boost::allocate_shared<DATA>( boost::shared_ptr<DATA> childData = boost::allocate_shared<DATA>(
tbb::scalable_allocator<DATA>(), visitorPre(child, *myData)); tbb::scalable_allocator<DATA>(), visitorPre(child, *myData));
tbb::task* childTask = ctg.run(PreOrderTask(child, childData, visitorPre, visitorPost,
new (allocate_child()) PreOrderTask(child, childData, visitorPre, visitorPost, problemSizeThreshold, ctg, overThreshold));
problemSizeThreshold, overThreshold);
if (firstChild)
childTasks.push_back(*childTask);
else
firstChild = childTask;
} }
ctg.wait();
// If we have child tasks, start subtasks and wait for them to complete // Allocate post-order task as a continuation
set_ref_count((int)treeNode->children.size()); isPostOrderPhase = true;
spawn(childTasks); tg.run(*this);
return firstChild;
} }
else else
{ {
// Run the post-order visitor in this task if we have no children // Run the post-order visitor in this task if we have no children
(void) visitorPost(treeNode, *myData); (void) visitorPost(treeNode, *myData);
return nullptr;
} }
} }
else else
{ {
// Process this node and its children in this task // Process this node and its children in this task
processNodeRecursively(treeNode, *myData); processNodeRecursively(treeNode, *myData);
return nullptr;
} }
} }
} }
void processNodeRecursively(const boost::shared_ptr<NODE>& node, DATA& myData) void processNodeRecursively(const boost::shared_ptr<NODE>& node, DATA& myData) const
{ {
for(const boost::shared_ptr<NODE>& child: node->children) for(const boost::shared_ptr<NODE>& child: node->children)
{ {
@ -131,7 +122,7 @@ namespace gtsam {
/* ************************************************************************* */ /* ************************************************************************* */
template<typename ROOTS, typename NODE, typename DATA, typename VISITOR_PRE, typename VISITOR_POST> template<typename ROOTS, typename NODE, typename DATA, typename VISITOR_PRE, typename VISITOR_POST>
class RootTask : public tbb::task class RootTask
{ {
public: public:
const ROOTS& roots; const ROOTS& roots;
@ -139,38 +130,31 @@ namespace gtsam {
VISITOR_PRE& visitorPre; VISITOR_PRE& visitorPre;
VISITOR_POST& visitorPost; VISITOR_POST& visitorPost;
int problemSizeThreshold; int problemSizeThreshold;
tbb::task_group& tg;
RootTask(const ROOTS& roots, DATA& myData, VISITOR_PRE& visitorPre, VISITOR_POST& visitorPost, RootTask(const ROOTS& roots, DATA& myData, VISITOR_PRE& visitorPre, VISITOR_POST& visitorPost,
int problemSizeThreshold) : int problemSizeThreshold, tbb::task_group& tg) :
roots(roots), myData(myData), visitorPre(visitorPre), visitorPost(visitorPost), roots(roots), myData(myData), visitorPre(visitorPre), visitorPost(visitorPost),
problemSizeThreshold(problemSizeThreshold) {} problemSizeThreshold(problemSizeThreshold), tg(tg) {}
tbb::task* execute() override void operator()() const
{ {
typedef PreOrderTask<NODE, DATA, VISITOR_PRE, VISITOR_POST> PreOrderTask; typedef PreOrderTask<NODE, DATA, VISITOR_PRE, VISITOR_POST> PreOrderTask;
// Create data and tasks for our children // Create data and tasks for our children
tbb::task_list tasks;
for(const boost::shared_ptr<NODE>& root: roots) for(const boost::shared_ptr<NODE>& root: roots)
{ {
boost::shared_ptr<DATA> rootData = boost::allocate_shared<DATA>(tbb::scalable_allocator<DATA>(), visitorPre(root, myData)); boost::shared_ptr<DATA> rootData = boost::allocate_shared<DATA>(tbb::scalable_allocator<DATA>(), visitorPre(root, myData));
tasks.push_back(*new(allocate_child()) tg.run(PreOrderTask(root, rootData, visitorPre, visitorPost, problemSizeThreshold, tg));
PreOrderTask(root, rootData, visitorPre, visitorPost, problemSizeThreshold));
} }
// Set TBB ref count
set_ref_count(1 + (int) roots.size());
// Spawn tasks
spawn_and_wait_for_all(tasks);
// Return nullptr
return nullptr;
} }
}; };
template<typename NODE, typename ROOTS, typename DATA, typename VISITOR_PRE, typename VISITOR_POST> template<typename NODE, typename ROOTS, typename DATA, typename VISITOR_PRE, typename VISITOR_POST>
RootTask<ROOTS, NODE, DATA, VISITOR_PRE, VISITOR_POST>& void CreateRootTask(const ROOTS& roots, DATA& rootData, VISITOR_PRE& visitorPre, VISITOR_POST& visitorPost, int problemSizeThreshold)
CreateRootTask(const ROOTS& roots, DATA& rootData, VISITOR_PRE& visitorPre, VISITOR_POST& visitorPost, int problemSizeThreshold)
{ {
typedef RootTask<ROOTS, NODE, DATA, VISITOR_PRE, VISITOR_POST> RootTask; typedef RootTask<ROOTS, NODE, DATA, VISITOR_PRE, VISITOR_POST> RootTask;
return *new(tbb::task::allocate_root()) RootTask(roots, rootData, visitorPre, visitorPost, problemSizeThreshold); tbb::task_group tg;
} tg.run_and_wait(RootTask(roots, rootData, visitorPre, visitorPost, problemSizeThreshold, tg));
}
} }

8
gtsam/base/types.h
View File

@ -34,6 +34,14 @@
#include <tbb/scalable_allocator.h> #include <tbb/scalable_allocator.h>
#endif #endif
#if defined(__GNUC__) || defined(__clang__)
#define GTSAM_DEPRECATED __attribute__((deprecated))
#elif defined(_MSC_VER)
#define GTSAM_DEPRECATED __declspec(deprecated)
#else
#define GTSAM_DEPRECATED
#endif
#ifdef GTSAM_USE_EIGEN_MKL_OPENMP #ifdef GTSAM_USE_EIGEN_MKL_OPENMP
#include <omp.h> #include <omp.h>
#endif #endif

3
gtsam/config.h.in
View File

@ -80,3 +80,6 @@
// Whether to use the system installed Metis instead of the provided one // Whether to use the system installed Metis instead of the provided one
#cmakedefine GTSAM_USE_SYSTEM_METIS #cmakedefine GTSAM_USE_SYSTEM_METIS
// Toggle switch for BetweenFactor jacobian computation
#cmakedefine GTSAM_SLOW_BUT_CORRECT_BETWEENFACTOR

12
gtsam/discrete/DiscreteBayesNet.cpp
View File

@ -54,20 +54,20 @@ namespace gtsam {
} }
/* ************************************************************************* */ /* ************************************************************************* */
DiscreteFactor::sharedValues DiscreteBayesNet::optimize() const { DiscreteFactor::Values DiscreteBayesNet::optimize() const {
// solve each node in turn in topological sort order (parents first) // solve each node in turn in topological sort order (parents first)
DiscreteFactor::sharedValues result(new DiscreteFactor::Values()); DiscreteFactor::Values result;
for (auto conditional: boost::adaptors::reverse(*this)) for (auto conditional: boost::adaptors::reverse(*this))
conditional->solveInPlace(*result); conditional->solveInPlace(&result);
return result; return result;
} }
/* ************************************************************************* */ /* ************************************************************************* */
DiscreteFactor::sharedValues DiscreteBayesNet::sample() const { DiscreteFactor::Values DiscreteBayesNet::sample() const {
// sample each node in turn in topological sort order (parents first) // sample each node in turn in topological sort order (parents first)
DiscreteFactor::sharedValues result(new DiscreteFactor::Values()); DiscreteFactor::Values result;
for (auto conditional: boost::adaptors::reverse(*this)) for (auto conditional: boost::adaptors::reverse(*this))
conditional->sampleInPlace(*result); conditional->sampleInPlace(&result);
return result; return result;
} }

4
gtsam/discrete/DiscreteBayesNet.h
View File

@ -83,10 +83,10 @@ namespace gtsam {
/** /**
* Solve the DiscreteBayesNet by back-substitution * Solve the DiscreteBayesNet by back-substitution
*/ */
DiscreteFactor::sharedValues optimize() const; DiscreteFactor::Values optimize() const;
/** Do ancestral sampling */ /** Do ancestral sampling */
DiscreteFactor::sharedValues sample() const; DiscreteFactor::Values sample() const;
///@} ///@}

14
gtsam/discrete/DiscreteConditional.cpp
View File

@ -35,7 +35,7 @@ using namespace std;
namespace gtsam { namespace gtsam {
// Instantiate base class // Instantiate base class
template class Conditional<DecisionTreeFactor, DiscreteConditional> ; template class GTSAM_EXPORT Conditional<DecisionTreeFactor, DiscreteConditional> ;
/* ******************************************************************************** */ /* ******************************************************************************** */
DiscreteConditional::DiscreteConditional(const size_t nrFrontals, DiscreteConditional::DiscreteConditional(const size_t nrFrontals,
@ -117,9 +117,9 @@ Potentials::ADT DiscreteConditional::choose(const Values& parentsValues) const {
} }
/* ******************************************************************************** */ /* ******************************************************************************** */
void DiscreteConditional::solveInPlace(Values& values) const { void DiscreteConditional::solveInPlace(Values* values) const {
// TODO: Abhijit asks: is this really the fastest way? He thinks it is. // TODO: Abhijit asks: is this really the fastest way? He thinks it is.
ADT pFS = choose(values); // P(F|S=parentsValues) ADT pFS = choose(*values); // P(F|S=parentsValues)
// Initialize // Initialize
Values mpe; Values mpe;
@ -145,16 +145,16 @@ void DiscreteConditional::solveInPlace(Values& values) const {
//set values (inPlace) to mpe //set values (inPlace) to mpe
for(Key j: frontals()) { for(Key j: frontals()) {
values[j] = mpe[j]; (*values)[j] = mpe[j];
} }
} }
/* ******************************************************************************** */ /* ******************************************************************************** */
void DiscreteConditional::sampleInPlace(Values& values) const { void DiscreteConditional::sampleInPlace(Values* values) const {
assert(nrFrontals() == 1); assert(nrFrontals() == 1);
Key j = (firstFrontalKey()); Key j = (firstFrontalKey());
size_t sampled = sample(values); // Sample variable size_t sampled = sample(*values); // Sample variable given parents
values[j] = sampled; // store result in partial solution (*values)[j] = sampled; // store result in partial solution
} }
/* ******************************************************************************** */ /* ******************************************************************************** */

5
gtsam/discrete/DiscreteConditional.h
View File

@ -45,7 +45,6 @@ public:
/** A map from keys to values.. /** A map from keys to values..
* TODO: Again, do we need this??? */ * TODO: Again, do we need this??? */
typedef Assignment<Key> Values; typedef Assignment<Key> Values;
typedef boost::shared_ptr<Values> sharedValues;
/// @name Standard Constructors /// @name Standard Constructors
/// @{ /// @{
@ -133,10 +132,10 @@ public:
/// @{ /// @{
/// solve a conditional, in place /// solve a conditional, in place
void solveInPlace(Values& parentsValues) const; void solveInPlace(Values* parentsValues) const;
/// sample in place, stores result in partial solution /// sample in place, stores result in partial solution
void sampleInPlace(Values& parentsValues) const; void sampleInPlace(Values* parentsValues) const;
/// @} /// @}

1
gtsam/discrete/DiscreteFactor.h
View File

@ -51,7 +51,6 @@ public:
* the new class DiscreteValue, as the varible's type (domain) * the new class DiscreteValue, as the varible's type (domain)
*/ */
typedef Assignment<Key> Values; typedef Assignment<Key> Values;
typedef boost::shared_ptr<Values> sharedValues;
public: public:

2
gtsam/discrete/DiscreteFactorGraph.cpp
View File

@ -94,7 +94,7 @@ namespace gtsam {
// } // }
/* ************************************************************************* */ /* ************************************************************************* */
DiscreteFactor::sharedValues DiscreteFactorGraph::optimize() const DiscreteFactorGraph::Values DiscreteFactorGraph::optimize() const
{ {
gttic(DiscreteFactorGraph_optimize); gttic(DiscreteFactorGraph_optimize);
return BaseEliminateable::eliminateSequential()->optimize(); return BaseEliminateable::eliminateSequential()->optimize();

19
gtsam/discrete/DiscreteFactorGraph.h
View File

@ -74,7 +74,6 @@ public:
/** A map from keys to values */ /** A map from keys to values */
typedef KeyVector Indices; typedef KeyVector Indices;
typedef Assignment<Key> Values; typedef Assignment<Key> Values;
typedef boost::shared_ptr<Values> sharedValues;
/** Default constructor */ /** Default constructor */
DiscreteFactorGraph() {} DiscreteFactorGraph() {}
@ -101,25 +100,27 @@ public:
/// @} /// @}
template<class SOURCE> // Add single key decision-tree factor.
template <class SOURCE>
void add(const DiscreteKey& j, SOURCE table) { void add(const DiscreteKey& j, SOURCE table) {
DiscreteKeys keys; DiscreteKeys keys;
keys.push_back(j); keys.push_back(j);
push_back(boost::make_shared<DecisionTreeFactor>(keys, table)); emplace_shared<DecisionTreeFactor>(keys, table);
} }
template<class SOURCE> // Add binary key decision-tree factor.
template <class SOURCE>
void add(const DiscreteKey& j1, const DiscreteKey& j2, SOURCE table) { void add(const DiscreteKey& j1, const DiscreteKey& j2, SOURCE table) {
DiscreteKeys keys; DiscreteKeys keys;
keys.push_back(j1); keys.push_back(j1);
keys.push_back(j2); keys.push_back(j2);
push_back(boost::make_shared<DecisionTreeFactor>(keys, table)); emplace_shared<DecisionTreeFactor>(keys, table);
} }
/** add shared discreteFactor immediately from arguments */ // Add shared discreteFactor immediately from arguments.
template<class SOURCE> template <class SOURCE>
void add(const DiscreteKeys& keys, SOURCE table) { void add(const DiscreteKeys& keys, SOURCE table) {
push_back(boost::make_shared<DecisionTreeFactor>(keys, table)); emplace_shared<DecisionTreeFactor>(keys, table);
} }
/** Return the set of variables involved in the factors (set union) */ /** Return the set of variables involved in the factors (set union) */
@ -140,7 +141,7 @@ public:
* the dense elimination function specified in \c function, * the dense elimination function specified in \c function,
* followed by back-substitution resulting from elimination. Is equivalent * followed by back-substitution resulting from elimination. Is equivalent
* to calling graph.eliminateSequential()->optimize(). */ * to calling graph.eliminateSequential()->optimize(). */
DiscreteFactor::sharedValues optimize() const; Values optimize() const;
// /** Permute the variables in the factors */ // /** Permute the variables in the factors */

12
gtsam/discrete/tests/testDiscreteBayesNet.cpp
View File

@ -104,12 +104,12 @@ TEST(DiscreteBayesNet, Asia) {
EXPECT(assert_equal(expected2, *chordal->back())); EXPECT(assert_equal(expected2, *chordal->back()));
// solve // solve
DiscreteFactor::sharedValues actualMPE = chordal->optimize(); auto actualMPE = chordal->optimize();
DiscreteFactor::Values expectedMPE; DiscreteFactor::Values expectedMPE;
insert(expectedMPE)(Asia.first, 0)(Dyspnea.first, 0)(XRay.first, 0)( insert(expectedMPE)(Asia.first, 0)(Dyspnea.first, 0)(XRay.first, 0)(
Tuberculosis.first, 0)(Smoking.first, 0)(Either.first, 0)( Tuberculosis.first, 0)(Smoking.first, 0)(Either.first, 0)(
LungCancer.first, 0)(Bronchitis.first, 0); LungCancer.first, 0)(Bronchitis.first, 0);
EXPECT(assert_equal(expectedMPE, *actualMPE)); EXPECT(assert_equal(expectedMPE, actualMPE));
// add evidence, we were in Asia and we have dyspnea // add evidence, we were in Asia and we have dyspnea
fg.add(Asia, "0 1"); fg.add(Asia, "0 1");
@ -117,12 +117,12 @@ TEST(DiscreteBayesNet, Asia) {
// solve again, now with evidence // solve again, now with evidence
DiscreteBayesNet::shared_ptr chordal2 = fg.eliminateSequential(ordering); DiscreteBayesNet::shared_ptr chordal2 = fg.eliminateSequential(ordering);
DiscreteFactor::sharedValues actualMPE2 = chordal2->optimize(); auto actualMPE2 = chordal2->optimize();
DiscreteFactor::Values expectedMPE2; DiscreteFactor::Values expectedMPE2;
insert(expectedMPE2)(Asia.first, 1)(Dyspnea.first, 1)(XRay.first, 0)( insert(expectedMPE2)(Asia.first, 1)(Dyspnea.first, 1)(XRay.first, 0)(
Tuberculosis.first, 0)(Smoking.first, 1)(Either.first, 0)( Tuberculosis.first, 0)(Smoking.first, 1)(Either.first, 0)(
LungCancer.first, 0)(Bronchitis.first, 1); LungCancer.first, 0)(Bronchitis.first, 1);
EXPECT(assert_equal(expectedMPE2, *actualMPE2)); EXPECT(assert_equal(expectedMPE2, actualMPE2));
// now sample from it // now sample from it
DiscreteFactor::Values expectedSample; DiscreteFactor::Values expectedSample;
@ -130,8 +130,8 @@ TEST(DiscreteBayesNet, Asia) {
insert(expectedSample)(Asia.first, 1)(Dyspnea.first, 1)(XRay.first, 1)( insert(expectedSample)(Asia.first, 1)(Dyspnea.first, 1)(XRay.first, 1)(
Tuberculosis.first, 0)(Smoking.first, 1)(Either.first, 1)( Tuberculosis.first, 0)(Smoking.first, 1)(Either.first, 1)(
LungCancer.first, 1)(Bronchitis.first, 0); LungCancer.first, 1)(Bronchitis.first, 0);
DiscreteFactor::sharedValues actualSample = chordal2->sample(); auto actualSample = chordal2->sample();
EXPECT(assert_equal(expectedSample, *actualSample)); EXPECT(assert_equal(expectedSample, actualSample));
} }
/* ************************************************************************* */ /* ************************************************************************* */

16
gtsam/discrete/tests/testDiscreteFactorGraph.cpp
View File

@ -169,8 +169,8 @@ TEST( DiscreteFactorGraph, test)
// Test optimization // Test optimization
DiscreteFactor::Values expectedValues; DiscreteFactor::Values expectedValues;
insert(expectedValues)(0, 0)(1, 0)(2, 0); insert(expectedValues)(0, 0)(1, 0)(2, 0);
DiscreteFactor::sharedValues actualValues = graph.optimize(); auto actualValues = graph.optimize();
EXPECT(assert_equal(expectedValues, *actualValues)); EXPECT(assert_equal(expectedValues, actualValues));
} }
/* ************************************************************************* */ /* ************************************************************************* */
@ -186,11 +186,11 @@ TEST( DiscreteFactorGraph, testMPE)
// graph.product().print(); // graph.product().print();
// DiscreteSequentialSolver(graph).eliminate()->print(); // DiscreteSequentialSolver(graph).eliminate()->print();
DiscreteFactor::sharedValues actualMPE = graph.optimize(); auto actualMPE = graph.optimize();
DiscreteFactor::Values expectedMPE; DiscreteFactor::Values expectedMPE;
insert(expectedMPE)(0, 0)(1, 1)(2, 1); insert(expectedMPE)(0, 0)(1, 1)(2, 1);
EXPECT(assert_equal(expectedMPE, *actualMPE)); EXPECT(assert_equal(expectedMPE, actualMPE));
} }
/* ************************************************************************* */ /* ************************************************************************* */
@ -216,8 +216,8 @@ TEST( DiscreteFactorGraph, testMPE_Darwiche09book_p244)
// Use the solver machinery. // Use the solver machinery.
DiscreteBayesNet::shared_ptr chordal = graph.eliminateSequential(); DiscreteBayesNet::shared_ptr chordal = graph.eliminateSequential();
DiscreteFactor::sharedValues actualMPE = chordal->optimize(); auto actualMPE = chordal->optimize();
EXPECT(assert_equal(expectedMPE, *actualMPE)); EXPECT(assert_equal(expectedMPE, actualMPE));
// DiscreteConditional::shared_ptr root = chordal->back(); // DiscreteConditional::shared_ptr root = chordal->back();
// EXPECT_DOUBLES_EQUAL(0.4, (*root)(*actualMPE), 1e-9); // EXPECT_DOUBLES_EQUAL(0.4, (*root)(*actualMPE), 1e-9);
@ -244,8 +244,8 @@ ETree::shared_ptr eTree = ETree::Create(graph, structure);
// eliminate normally and check solution // eliminate normally and check solution
DiscreteBayesNet::shared_ptr bayesNet = eTree->eliminate(&EliminateDiscrete); DiscreteBayesNet::shared_ptr bayesNet = eTree->eliminate(&EliminateDiscrete);
// bayesNet->print(">>>>>>>>>>>>>> Bayes Net <<<<<<<<<<<<<<<<<<"); // bayesNet->print(">>>>>>>>>>>>>> Bayes Net <<<<<<<<<<<<<<<<<<");
DiscreteFactor::sharedValues actualMPE = optimize(*bayesNet); auto actualMPE = optimize(*bayesNet);
EXPECT(assert_equal(expectedMPE, *actualMPE)); EXPECT(assert_equal(expectedMPE, actualMPE));
// Approximate and check solution // Approximate and check solution
// DiscreteBayesNet::shared_ptr approximateNet = eTree->approximate(); // DiscreteBayesNet::shared_ptr approximateNet = eTree->approximate();

46
gtsam/geometry/Cal3Fisheye.cpp
View File

@ -46,9 +46,9 @@ double Cal3Fisheye::Scaling(double r) {
/* ************************************************************************* */ /* ************************************************************************* */
Point2 Cal3Fisheye::uncalibrate(const Point2& p, OptionalJacobian<2, 9> H1, Point2 Cal3Fisheye::uncalibrate(const Point2& p, OptionalJacobian<2, 9> H1,
OptionalJacobian<2, 2> H2) const { OptionalJacobian<2, 2> H2) const {
const double xi = p.x(), yi = p.y(); const double xi = p.x(), yi = p.y(), zi = 1;
const double r2 = xi * xi + yi * yi, r = sqrt(r2); const double r2 = xi * xi + yi * yi, r = sqrt(r2);
const double t = atan(r); const double t = atan2(r, zi);
const double t2 = t * t, t4 = t2 * t2, t6 = t2 * t4, t8 = t4 * t4; const double t2 = t * t, t4 = t2 * t2, t6 = t2 * t4, t8 = t4 * t4;
Vector5 K, T; Vector5 K, T;
K << 1, k1_, k2_, k3_, k4_; K << 1, k1_, k2_, k3_, k4_;
@ -76,28 +76,32 @@ Point2 Cal3Fisheye::uncalibrate(const Point2& p, OptionalJacobian<2, 9> H1,
// Derivative for points in intrinsic coords (2 by 2) // Derivative for points in intrinsic coords (2 by 2)
if (H2) { if (H2) {
const double dtd_dt = if (r2==0) {
1 + 3 * k1_ * t2 + 5 * k2_ * t4 + 7 * k3_ * t6 + 9 * k4_ * t8; *H2 = DK;
const double dt_dr = 1 / (1 + r2); } else {
const double rinv = 1 / r; const double dtd_dt =
const double dr_dxi = xi * rinv; 1 + 3 * k1_ * t2 + 5 * k2_ * t4 + 7 * k3_ * t6 + 9 * k4_ * t8;
const double dr_dyi = yi * rinv; const double R2 = r2 + zi*zi;
const double dtd_dxi = dtd_dt * dt_dr * dr_dxi; const double dt_dr = zi / R2;
const double dtd_dyi = dtd_dt * dt_dr * dr_dyi; const double rinv = 1 / r;
const double dr_dxi = xi * rinv;
const double dr_dyi = yi * rinv;
const double dtd_dr = dtd_dt * dt_dr;
const double c2 = dr_dxi * dr_dxi;
const double s2 = dr_dyi * dr_dyi;
const double cs = dr_dxi * dr_dyi;
const double td = t * K.dot(T); const double dxd_dxi = dtd_dr * c2 + s * (1 - c2);
const double rrinv = 1 / r2; const double dxd_dyi = (dtd_dr - s) * cs;
const double dxd_dxi = const double dyd_dxi = dxd_dyi;
dtd_dxi * dr_dxi + td * rinv - td * xi * rrinv * dr_dxi; const double dyd_dyi = dtd_dr * s2 + s * (1 - s2);
const double dxd_dyi = dtd_dyi * dr_dxi - td * xi * rrinv * dr_dyi;
const double dyd_dxi = dtd_dxi * dr_dyi - td * yi * rrinv * dr_dxi;
const double dyd_dyi =
dtd_dyi * dr_dyi + td * rinv - td * yi * rrinv * dr_dyi;
Matrix2 DR; Matrix2 DR;
DR << dxd_dxi, dxd_dyi, dyd_dxi, dyd_dyi; DR << dxd_dxi, dxd_dyi, dyd_dxi, dyd_dyi;
*H2 = DK * DR; *H2 = DK * DR;
}
} }
return uv; return uv;

10
gtsam/geometry/PinholeCamera.h
View File

@ -312,6 +312,16 @@ public:
return range(camera.pose(), Dcamera, Dother); return range(camera.pose(), Dcamera, Dother);
} }
/// for Linear Triangulation
Matrix34 cameraProjectionMatrix() const {
return K_.K() * PinholeBase::pose().inverse().matrix().block(0, 0, 3, 4);
}
/// for Nonlinear Triangulation
Vector defaultErrorWhenTriangulatingBehindCamera() const {
return Eigen::Matrix<double,traits<Point2>::dimension,1>::Constant(2.0 * K_.fx());;
}
private: private:
/** Serialization function */ /** Serialization function */

20
gtsam/geometry/PinholePose.h
View File

@ -35,7 +35,7 @@ class GTSAM_EXPORT PinholeBaseK: public PinholeBase {
private: private:
GTSAM_CONCEPT_MANIFOLD_TYPE(CALIBRATION); GTSAM_CONCEPT_MANIFOLD_TYPE(CALIBRATION)
// Get dimensions of calibration type at compile time // Get dimensions of calibration type at compile time
static const int DimK = FixedDimension<CALIBRATION>::value; static const int DimK = FixedDimension<CALIBRATION>::value;
@ -121,6 +121,13 @@ public:
return _project(pw, Dpose, Dpoint, Dcal); return _project(pw, Dpose, Dpoint, Dcal);
} }
/// project a 3D point from world coordinates into the image
Point2 reprojectionError(const Point3& pw, const Point2& measured, OptionalJacobian<2, 6> Dpose = boost::none,
OptionalJacobian<2, 3> Dpoint = boost::none,
OptionalJacobian<2, DimK> Dcal = boost::none) const {
return Point2(_project(pw, Dpose, Dpoint, Dcal) - measured);
}
/// project a point at infinity from world coordinates into the image /// project a point at infinity from world coordinates into the image
Point2 project(const Unit3& pw, OptionalJacobian<2, 6> Dpose = boost::none, Point2 project(const Unit3& pw, OptionalJacobian<2, 6> Dpose = boost::none,
OptionalJacobian<2, 2> Dpoint = boost::none, OptionalJacobian<2, 2> Dpoint = boost::none,
@ -159,7 +166,6 @@ public:
return result; return result;
} }
/// backproject a 2-dimensional point to a 3-dimensional point at infinity /// backproject a 2-dimensional point to a 3-dimensional point at infinity
Unit3 backprojectPointAtInfinity(const Point2& p) const { Unit3 backprojectPointAtInfinity(const Point2& p) const {
const Point2 pn = calibration().calibrate(p); const Point2 pn = calibration().calibrate(p);
@ -410,6 +416,16 @@ public:
return PinholePose(); // assumes that the default constructor is valid return PinholePose(); // assumes that the default constructor is valid
} }
/// for Linear Triangulation
Matrix34 cameraProjectionMatrix() const {
Matrix34 P = Matrix34(PinholeBase::pose().inverse().matrix().block(0, 0, 3, 4));
return K_->K() * P;
}
/// for Nonlinear Triangulation
Vector defaultErrorWhenTriangulatingBehindCamera() const {
return Eigen::Matrix<double,traits<Point2>::dimension,1>::Constant(2.0 * K_->fx());;
}
/// @} /// @}
private: private:

2
gtsam/geometry/Pose2.cpp
View File

@ -28,7 +28,7 @@ using namespace std;
namespace gtsam { namespace gtsam {
/** instantiate concept checks */ /** instantiate concept checks */
GTSAM_CONCEPT_POSE_INST(Pose2); GTSAM_CONCEPT_POSE_INST(Pose2)
static const Rot2 R_PI_2(Rot2::fromCosSin(0., 1.)); static const Rot2 R_PI_2(Rot2::fromCosSin(0., 1.));

54
gtsam/geometry/Pose3.cpp
View File

@ -30,7 +30,7 @@ using std::vector;
using Point3Pairs = vector<Point3Pair>; using Point3Pairs = vector<Point3Pair>;
/** instantiate concept checks */ /** instantiate concept checks */
GTSAM_CONCEPT_POSE_INST(Pose3); GTSAM_CONCEPT_POSE_INST(Pose3)
/* ************************************************************************* */ /* ************************************************************************* */
Pose3::Pose3(const Pose2& pose2) : Pose3::Pose3(const Pose2& pose2) :
@ -63,6 +63,46 @@ Matrix6 Pose3::AdjointMap() const {
return adj; return adj;
} }
/* ************************************************************************* */
// Calculate AdjointMap applied to xi_b, with Jacobians
Vector6 Pose3::Adjoint(const Vector6& xi_b, OptionalJacobian<6, 6> H_pose,
OptionalJacobian<6, 6> H_xib) const {
const Matrix6 Ad = AdjointMap();
// Jacobians
// D1 Ad_T(xi_b) = D1 Ad_T Ad_I(xi_b) = Ad_T * D1 Ad_I(xi_b) = Ad_T * ad_xi_b
// D2 Ad_T(xi_b) = Ad_T
// See docs/math.pdf for more details.
// In D1 calculation, we could be more efficient by writing it out, but do not
// for readability
if (H_pose) *H_pose = -Ad * adjointMap(xi_b);
if (H_xib) *H_xib = Ad;
return Ad * xi_b;
}
/* ************************************************************************* */
/// The dual version of Adjoint
Vector6 Pose3::AdjointTranspose(const Vector6& x, OptionalJacobian<6, 6> H_pose,
OptionalJacobian<6, 6> H_x) const {
const Matrix6 Ad = AdjointMap();
const Vector6 AdTx = Ad.transpose() * x;
// Jacobians
// See docs/math.pdf for more details.
if (H_pose) {
const auto w_T_hat = skewSymmetric(AdTx.head<3>()),
v_T_hat = skewSymmetric(AdTx.tail<3>());
*H_pose << w_T_hat, v_T_hat, //
/* */ v_T_hat, Z_3x3;
}
if (H_x) {
*H_x = Ad.transpose();
}
return AdTx;
}
/* ************************************************************************* */ /* ************************************************************************* */
Matrix6 Pose3::adjointMap(const Vector6& xi) { Matrix6 Pose3::adjointMap(const Vector6& xi) {
Matrix3 w_hat = skewSymmetric(xi(0), xi(1), xi(2)); Matrix3 w_hat = skewSymmetric(xi(0), xi(1), xi(2));
@ -75,7 +115,7 @@ Matrix6 Pose3::adjointMap(const Vector6& xi) {
/* ************************************************************************* */ /* ************************************************************************* */
Vector6 Pose3::adjoint(const Vector6& xi, const Vector6& y, Vector6 Pose3::adjoint(const Vector6& xi, const Vector6& y,
OptionalJacobian<6, 6> Hxi) { OptionalJacobian<6, 6> Hxi, OptionalJacobian<6, 6> H_y) {
if (Hxi) { if (Hxi) {
Hxi->setZero(); Hxi->setZero();
for (int i = 0; i < 6; ++i) { for (int i = 0; i < 6; ++i) {
@ -86,12 +126,14 @@ Vector6 Pose3::adjoint(const Vector6& xi, const Vector6& y,
Hxi->col(i) = Gi * y; Hxi->col(i) = Gi * y;
} }
} }
return adjointMap(xi) * y; const Matrix6& ad_xi = adjointMap(xi);
if (H_y) *H_y = ad_xi;
return ad_xi * y;
} }
/* ************************************************************************* */ /* ************************************************************************* */
Vector6 Pose3::adjointTranspose(const Vector6& xi, const Vector6& y, Vector6 Pose3::adjointTranspose(const Vector6& xi, const Vector6& y,
OptionalJacobian<6, 6> Hxi) { OptionalJacobian<6, 6> Hxi, OptionalJacobian<6, 6> H_y) {
if (Hxi) { if (Hxi) {
Hxi->setZero(); Hxi->setZero();
for (int i = 0; i < 6; ++i) { for (int i = 0; i < 6; ++i) {
@ -102,7 +144,9 @@ Vector6 Pose3::adjointTranspose(const Vector6& xi, const Vector6& y,
Hxi->col(i) = GTi * y; Hxi->col(i) = GTi * y;
} }
} }
return adjointMap(xi).transpose() * y; const Matrix6& adT_xi = adjointMap(xi).transpose();
if (H_y) *H_y = adT_xi;
return adT_xi * y;
} }
/* ************************************************************************* */ /* ************************************************************************* */

27
gtsam/geometry/Pose3.h
View File

@ -145,15 +145,22 @@ public:
* Calculate Adjoint map, transforming a twist in this pose's (i.e, body) frame to the world spatial frame * Calculate Adjoint map, transforming a twist in this pose's (i.e, body) frame to the world spatial frame
* Ad_pose is 6*6 matrix that when applied to twist xi \f$ [R_x,R_y,R_z,T_x,T_y,T_z] \f$, returns Ad_pose(xi) * Ad_pose is 6*6 matrix that when applied to twist xi \f$ [R_x,R_y,R_z,T_x,T_y,T_z] \f$, returns Ad_pose(xi)
*/ */
Matrix6 AdjointMap() const; /// FIXME Not tested - marked as incorrect Matrix6 AdjointMap() const;
/** /**
* Apply this pose's AdjointMap Ad_g to a twist \f$ \xi_b \f$, i.e. a body-fixed velocity, transforming it to the spatial frame * Apply this pose's AdjointMap Ad_g to a twist \f$ \xi_b \f$, i.e. a
* body-fixed velocity, transforming it to the spatial frame
* \f$ \xi^s = g*\xi^b*g^{-1} = Ad_g * \xi^b \f$ * \f$ \xi^s = g*\xi^b*g^{-1} = Ad_g * \xi^b \f$
* Note that H_xib = AdjointMap()
*/ */
Vector6 Adjoint(const Vector6& xi_b) const { Vector6 Adjoint(const Vector6& xi_b,
return AdjointMap() * xi_b; OptionalJacobian<6, 6> H_this = boost::none,
} /// FIXME Not tested - marked as incorrect OptionalJacobian<6, 6> H_xib = boost::none) const;
/// The dual version of Adjoint
Vector6 AdjointTranspose(const Vector6& x,
OptionalJacobian<6, 6> H_this = boost::none,
OptionalJacobian<6, 6> H_x = boost::none) const;
/** /**
* Compute the [ad(w,v)] operator as defined in [Kobilarov09siggraph], pg 11 * Compute the [ad(w,v)] operator as defined in [Kobilarov09siggraph], pg 11
@ -170,13 +177,14 @@ public:
* and its inverse transpose in the discrete Euler Poincare' (DEP) operator. * and its inverse transpose in the discrete Euler Poincare' (DEP) operator.
* *
*/ */
static Matrix6 adjointMap(const Vector6 &xi); static Matrix6 adjointMap(const Vector6& xi);
/** /**
* Action of the adjointMap on a Lie-algebra vector y, with optional derivatives * Action of the adjointMap on a Lie-algebra vector y, with optional derivatives
*/ */
static Vector6 adjoint(const Vector6 &xi, const Vector6 &y, static Vector6 adjoint(const Vector6& xi, const Vector6& y,
OptionalJacobian<6, 6> Hxi = boost::none); OptionalJacobian<6, 6> Hxi = boost::none,
OptionalJacobian<6, 6> H_y = boost::none);
// temporary fix for wrappers until case issue is resolved // temporary fix for wrappers until case issue is resolved
static Matrix6 adjointMap_(const Vector6 &xi) { return adjointMap(xi);} static Matrix6 adjointMap_(const Vector6 &xi) { return adjointMap(xi);}
@ -186,7 +194,8 @@ public:
* The dual version of adjoint action, acting on the dual space of the Lie-algebra vector space. * The dual version of adjoint action, acting on the dual space of the Lie-algebra vector space.
*/ */
static Vector6 adjointTranspose(const Vector6& xi, const Vector6& y, static Vector6 adjointTranspose(const Vector6& xi, const Vector6& y,
OptionalJacobian<6, 6> Hxi = boost::none); OptionalJacobian<6, 6> Hxi = boost::none,
OptionalJacobian<6, 6> H_y = boost::none);
/// Derivative of Expmap /// Derivative of Expmap
static Matrix6 ExpmapDerivative(const Vector6& xi); static Matrix6 ExpmapDerivative(const Vector6& xi);

24
gtsam/geometry/Quaternion.h
View File

@ -117,13 +117,23 @@ struct traits<QUATERNION_TYPE> {
omega = (-8. / 3. - 2. / 3. * qw) * q.vec(); omega = (-8. / 3. - 2. / 3. * qw) * q.vec();
} else { } else {
// Normal, away from zero case // Normal, away from zero case
_Scalar angle = 2 * acos(qw), s = sqrt(1 - qw * qw); if (qw > 0) {
// Important: convert to [-pi,pi] to keep error continuous _Scalar angle = 2 * acos(qw), s = sqrt(1 - qw * qw);
if (angle > M_PI) // Important: convert to [-pi,pi] to keep error continuous
angle -= twoPi; if (angle > M_PI)
else if (angle < -M_PI) angle -= twoPi;
angle += twoPi; else if (angle < -M_PI)
omega = (angle / s) * q.vec(); angle += twoPi;
omega = (angle / s) * q.vec();
} else {
// Make sure that we are using a canonical quaternion with w > 0
_Scalar angle = 2 * acos(-qw), s = sqrt(1 - qw * qw);
if (angle > M_PI)
angle -= twoPi;
else if (angle < -M_PI)
angle += twoPi;
omega = (angle / s) * -q.vec();
}
} }
if(H) *H = SO3::LogmapDerivative(omega.template cast<double>()); if(H) *H = SO3::LogmapDerivative(omega.template cast<double>());

56
gtsam/geometry/SO3.cpp
View File

@ -261,25 +261,59 @@ Vector3 SO3::Logmap(const SO3& Q, ChartJacobian H) {
// when trace == -1, i.e., when theta = +-pi, +-3pi, +-5pi, etc. // when trace == -1, i.e., when theta = +-pi, +-3pi, +-5pi, etc.
// we do something special // we do something special
if (tr + 1.0 < 1e-10) { if (tr + 1.0 < 1e-3) {
if (std::abs(R33 + 1.0) > 1e-5) if (R33 > R22 && R33 > R11) {
omega = (M_PI / sqrt(2.0 + 2.0 * R33)) * Vector3(R13, R23, 1.0 + R33); // R33 is the largest diagonal, a=3, b=1, c=2
else if (std::abs(R22 + 1.0) > 1e-5) const double W = R21 - R12;
omega = (M_PI / sqrt(2.0 + 2.0 * R22)) * Vector3(R12, 1.0 + R22, R32); const double Q1 = 2.0 + 2.0 * R33;
else const double Q2 = R31 + R13;
// if(std::abs(R.r1_.x()+1.0) > 1e-5) This is implicit const double Q3 = R23 + R32;
omega = (M_PI / sqrt(2.0 + 2.0 * R11)) * Vector3(1.0 + R11, R21, R31); const double r = sqrt(Q1);
const double one_over_r = 1 / r;
const double norm = sqrt(Q1*Q1 + Q2*Q2 + Q3*Q3 + W*W);
const double sgn_w = W < 0 ? -1.0 : 1.0;
const double mag = M_PI - (2 * sgn_w * W) / norm;
const double scale = 0.5 * one_over_r * mag;
omega = sgn_w * scale * Vector3(Q2, Q3, Q1);
} else if (R22 > R11) {
// R22 is the largest diagonal, a=2, b=3, c=1
const double W = R13 - R31;
const double Q1 = 2.0 + 2.0 * R22;
const double Q2 = R23 + R32;
const double Q3 = R12 + R21;
const double r = sqrt(Q1);
const double one_over_r = 1 / r;
const double norm = sqrt(Q1*Q1 + Q2*Q2 + Q3*Q3 + W*W);
const double sgn_w = W < 0 ? -1.0 : 1.0;
const double mag = M_PI - (2 * sgn_w * W) / norm;
const double scale = 0.5 * one_over_r * mag;
omega = sgn_w * scale * Vector3(Q3, Q1, Q2);
} else {
// R11 is the largest diagonal, a=1, b=2, c=3
const double W = R32 - R23;
const double Q1 = 2.0 + 2.0 * R11;
const double Q2 = R12 + R21;
const double Q3 = R31 + R13;
const double r = sqrt(Q1);
const double one_over_r = 1 / r;
const double norm = sqrt(Q1*Q1 + Q2*Q2 + Q3*Q3 + W*W);
const double sgn_w = W < 0 ? -1.0 : 1.0;
const double mag = M_PI - (2 * sgn_w * W) / norm;
const double scale = 0.5 * one_over_r * mag;
omega = sgn_w * scale * Vector3(Q1, Q2, Q3);
}
} else { } else {
double magnitude; double magnitude;
const double tr_3 = tr - 3.0; // always negative const double tr_3 = tr - 3.0; // could be non-negative if the matrix is off orthogonal
if (tr_3 < -1e-7) { if (tr_3 < -1e-6) {
// this is the normal case -1 < trace < 3
double theta = acos((tr - 1.0) / 2.0); double theta = acos((tr - 1.0) / 2.0);
magnitude = theta / (2.0 * sin(theta)); magnitude = theta / (2.0 * sin(theta));
} else { } else {
// when theta near 0, +-2pi, +-4pi, etc. (trace near 3.0) // when theta near 0, +-2pi, +-4pi, etc. (trace near 3.0)
// use Taylor expansion: theta \approx 1/2-(t-3)/12 + O((t-3)^2) // use Taylor expansion: theta \approx 1/2-(t-3)/12 + O((t-3)^2)
// see https://github.com/borglab/gtsam/issues/746 for details // see https://github.com/borglab/gtsam/issues/746 for details
magnitude = 0.5 - tr_3 / 12.0; magnitude = 0.5 - tr_3 / 12.0 + tr_3*tr_3/60.0;
} }
omega = magnitude * Vector3(R32 - R23, R13 - R31, R21 - R12); omega = magnitude * Vector3(R32 - R23, R13 - R31, R21 - R12);
} }

4
gtsam/geometry/Similarity3.cpp
View File

@ -40,8 +40,8 @@ static Point3Pairs subtractCentroids(const Point3Pairs &abPointPairs,
} }
/// Form inner products x and y and calculate scale. /// Form inner products x and y and calculate scale.
static const double calculateScale(const Point3Pairs &d_abPointPairs, static double calculateScale(const Point3Pairs &d_abPointPairs,
const Rot3 &aRb) { const Rot3 &aRb) {
double x = 0, y = 0; double x = 0, y = 0;
Point3 da, db; Point3 da, db;
for (const Point3Pair& d_abPair : d_abPointPairs) { for (const Point3Pair& d_abPair : d_abPointPairs) {

109
gtsam/geometry/SphericalCamera.cpp Normal file
View File

@ -0,0 +1,109 @@
/* ----------------------------------------------------------------------------
* 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 SphericalCamera.h
* @brief Calibrated camera with spherical projection
* @date Aug 26, 2021
* @author Luca Carlone
*/
#include <gtsam/geometry/SphericalCamera.h>
using namespace std;
namespace gtsam {
/* ************************************************************************* */
bool SphericalCamera::equals(const SphericalCamera& camera, double tol) const {
return pose_.equals(camera.pose(), tol);
}
/* ************************************************************************* */
void SphericalCamera::print(const string& s) const { pose_.print(s + ".pose"); }
/* ************************************************************************* */
pair<Unit3, bool> SphericalCamera::projectSafe(const Point3& pw) const {
const Point3 pc = pose().transformTo(pw);
Unit3 pu = Unit3::FromPoint3(pc);
return make_pair(pu, pc.norm() > 1e-8);
}
/* ************************************************************************* */
Unit3 SphericalCamera::project2(const Point3& pw, OptionalJacobian<2, 6> Dpose,
OptionalJacobian<2, 3> Dpoint) const {
Matrix36 Dtf_pose;
Matrix3 Dtf_point; // calculated by transformTo if needed
const Point3 pc =
pose().transformTo(pw, Dpose ? &Dtf_pose : 0, Dpoint ? &Dtf_point : 0);
if (pc.norm() <= 1e-8) throw("point cannot be at the center of the camera");
Matrix23 Dunit; // calculated by FromPoint3 if needed
Unit3 pu = Unit3::FromPoint3(Point3(pc), Dpoint ? &Dunit : 0);
if (Dpose) *Dpose = Dunit * Dtf_pose; // 2x3 * 3x6 = 2x6
if (Dpoint) *Dpoint = Dunit * Dtf_point; // 2x3 * 3x3 = 2x3
return pu;
}
/* ************************************************************************* */
Unit3 SphericalCamera::project2(const Unit3& pwu, OptionalJacobian<2, 6> Dpose,
OptionalJacobian<2, 2> Dpoint) const {
Matrix23 Dtf_rot;
Matrix2 Dtf_point; // calculated by transformTo if needed
const Unit3 pu = pose().rotation().unrotate(pwu, Dpose ? &Dtf_rot : 0,
Dpoint ? &Dtf_point : 0);
if (Dpose)
*Dpose << Dtf_rot, Matrix::Zero(2, 3); // 2x6 (translation part is zero)
if (Dpoint) *Dpoint = Dtf_point; // 2x2
return pu;
}
/* ************************************************************************* */
Point3 SphericalCamera::backproject(const Unit3& pu, const double depth) const {
return pose().transformFrom(depth * pu);
}
/* ************************************************************************* */
Unit3 SphericalCamera::backprojectPointAtInfinity(const Unit3& p) const {
return pose().rotation().rotate(p);
}
/* ************************************************************************* */
Unit3 SphericalCamera::project(const Point3& point,
OptionalJacobian<2, 6> Dcamera,
OptionalJacobian<2, 3> Dpoint) const {
return project2(point, Dcamera, Dpoint);
}
/* ************************************************************************* */
Vector2 SphericalCamera::reprojectionError(
const Point3& point, const Unit3& measured, OptionalJacobian<2, 6> Dpose,
OptionalJacobian<2, 3> Dpoint) const {
// project point
if (Dpose || Dpoint) {
Matrix26 H_project_pose;
Matrix23 H_project_point;
Matrix22 H_error;
Unit3 projected = project2(point, H_project_pose, H_project_point);
Vector2 error = measured.errorVector(projected, boost::none, H_error);
if (Dpose) *Dpose = H_error * H_project_pose;
if (Dpoint) *Dpoint = H_error * H_project_point;
return error;
} else {
return measured.errorVector(project2(point, Dpose, Dpoint));
}
}
/* ************************************************************************* */
} // namespace gtsam

225
gtsam/geometry/SphericalCamera.h Normal file
View File

@ -0,0 +1,225 @@
/* ----------------------------------------------------------------------------
* 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 SphericalCamera.h
* @brief Calibrated camera with spherical projection
* @date Aug 26, 2021
* @author Luca Carlone
*/
#pragma once
#include <gtsam/base/Manifold.h>
#include <gtsam/base/ThreadsafeException.h>
#include <gtsam/base/concepts.h>
#include <gtsam/dllexport.h>
#include <gtsam/geometry/BearingRange.h>
#include <gtsam/geometry/Pose3.h>
#include <gtsam/geometry/Unit3.h>
#include <boost/serialization/nvp.hpp>
namespace gtsam {
/**
* Empty calibration. Only needed to play well with other cameras
* (e.g., when templating functions wrt cameras), since other cameras
* have constuctors in the form ‘camera(pose,calibration)’
* @addtogroup geometry
* \nosubgrouping
*/
class GTSAM_EXPORT EmptyCal {
public:
enum { dimension = 0 };
EmptyCal() {}
virtual ~EmptyCal() = default;
using shared_ptr = boost::shared_ptr<EmptyCal>;
void print(const std::string& s) const {
std::cout << "empty calibration: " << s << std::endl;
}
};
/**
* A spherical camera class that has a Pose3 and measures bearing vectors.
* The camera has an ‘Empty’ calibration and the only 6 dof are the pose
* @addtogroup geometry
* \nosubgrouping
*/
class GTSAM_EXPORT SphericalCamera {
public:
enum { dimension = 6 };
typedef Unit3 Measurement;
typedef std::vector<Unit3> MeasurementVector;
typedef EmptyCal CalibrationType;
private:
Pose3 pose_; ///< 3D pose of camera
protected:
EmptyCal::shared_ptr emptyCal_;
public:
/// @}
/// @name Standard Constructors
/// @{
/// Default constructor
SphericalCamera()
: pose_(Pose3::identity()), emptyCal_(boost::make_shared<EmptyCal>()) {}
/// Constructor with pose
explicit SphericalCamera(const Pose3& pose)
: pose_(pose), emptyCal_(boost::make_shared<EmptyCal>()) {}
/// Constructor with empty intrinsics (needed for smart factors)
explicit SphericalCamera(const Pose3& pose,
const boost::shared_ptr<EmptyCal>& cal)
: pose_(pose), emptyCal_(boost::make_shared<EmptyCal>()) {}
/// @}
/// @name Advanced Constructors
/// @{
explicit SphericalCamera(const Vector& v) : pose_(Pose3::Expmap(v)) {}
/// Default destructor
virtual ~SphericalCamera() = default;
/// return shared pointer to calibration
const boost::shared_ptr<EmptyCal>& sharedCalibration() const {
return emptyCal_;
}
/// return calibration
const EmptyCal& calibration() const { return *emptyCal_; }
/// @}
/// @name Testable
/// @{
/// assert equality up to a tolerance
bool equals(const SphericalCamera& camera, double tol = 1e-9) const;
/// print
virtual void print(const std::string& s = "SphericalCamera") const;
/// @}
/// @name Standard Interface
/// @{
/// return pose, constant version
const Pose3& pose() const { return pose_; }
/// get rotation
const Rot3& rotation() const { return pose_.rotation(); }
/// get translation
const Point3& translation() const { return pose_.translation(); }
// /// return pose, with derivative
// const Pose3& getPose(OptionalJacobian<6, 6> H) const;
/// @}
/// @name Transformations and measurement functions
/// @{
/// Project a point into the image and check depth
std::pair<Unit3, bool> projectSafe(const Point3& pw) const;
/** Project point into the image
* (note: there is no CheiralityException for a spherical camera)
* @param point 3D point in world coordinates
* @return the intrinsic coordinates of the projected point
*/
Unit3 project2(const Point3& pw, OptionalJacobian<2, 6> Dpose = boost::none,
OptionalJacobian<2, 3> Dpoint = boost::none) const;
/** Project point into the image
* (note: there is no CheiralityException for a spherical camera)
* @param point 3D direction in world coordinates
* @return the intrinsic coordinates of the projected point
*/
Unit3 project2(const Unit3& pwu, OptionalJacobian<2, 6> Dpose = boost::none,
OptionalJacobian<2, 2> Dpoint = boost::none) const;
/// backproject a 2-dimensional point to a 3-dimensional point at given depth
Point3 backproject(const Unit3& p, const double depth) const;
/// backproject point at infinity
Unit3 backprojectPointAtInfinity(const Unit3& p) const;
/** Project point into the image
* (note: there is no CheiralityException for a spherical camera)
* @param point 3D point in world coordinates
* @return the intrinsic coordinates of the projected point
*/
Unit3 project(const Point3& point, OptionalJacobian<2, 6> Dpose = boost::none,
OptionalJacobian<2, 3> Dpoint = boost::none) const;
/** Compute reprojection error for a given 3D point in world coordinates
* @param point 3D point in world coordinates
* @return the tangent space error between the projection and the measurement
*/
Vector2 reprojectionError(const Point3& point, const Unit3& measured,
OptionalJacobian<2, 6> Dpose = boost::none,
OptionalJacobian<2, 3> Dpoint = boost::none) const;
/// @}
/// move a cameras according to d
SphericalCamera retract(const Vector6& d) const {
return SphericalCamera(pose().retract(d));
}
/// return canonical coordinate
Vector6 localCoordinates(const SphericalCamera& p) const {
return pose().localCoordinates(p.pose());
}
/// for Canonical
static SphericalCamera identity() {
return SphericalCamera(
Pose3::identity()); // assumes that the default constructor is valid
}
/// for Linear Triangulation
Matrix34 cameraProjectionMatrix() const {
return Matrix34(pose_.inverse().matrix().block(0, 0, 3, 4));
}
/// for Nonlinear Triangulation
Vector defaultErrorWhenTriangulatingBehindCamera() const {
return Eigen::Matrix<double, traits<Point2>::dimension, 1>::Constant(0.0);
}
/// @deprecated
size_t dim() const { return 6; }
/// @deprecated
static size_t Dim() { return 6; }
private:
/** Serialization function */
friend class boost::serialization::access;
template <class Archive>
void serialize(Archive& ar, const unsigned int /*version*/) {
ar& BOOST_SERIALIZATION_NVP(pose_);
}
};
// end of class SphericalCamera
template <>
struct traits<SphericalCamera> : public internal::LieGroup<Pose3> {};
template <>
struct traits<const SphericalCamera> : public internal::LieGroup<Pose3> {};
} // namespace gtsam

5
gtsam/geometry/StereoCamera.h
View File

@ -170,6 +170,11 @@ public:
OptionalJacobian<3, 3> H2 = boost::none, OptionalJacobian<3, 0> H3 = OptionalJacobian<3, 3> H2 = boost::none, OptionalJacobian<3, 0> H3 =
boost::none) const; boost::none) const;
/// for Nonlinear Triangulation
Vector defaultErrorWhenTriangulatingBehindCamera() const {
return Eigen::Matrix<double,traits<Measurement>::dimension,1>::Constant(2.0 * K_->fx());;
}
/// @} /// @}
private: private:

2
gtsam/geometry/concepts.h
View File

@ -72,5 +72,5 @@ private:
/** Pose Concept macros */ /** Pose Concept macros */
#define GTSAM_CONCEPT_POSE_INST(T) template class gtsam::PoseConcept<T>; #define GTSAM_CONCEPT_POSE_INST(T) template class gtsam::PoseConcept<T>;
#define GTSAM_CONCEPT_POSE_TYPE(T) typedef gtsam::PoseConcept<T> _gtsam_PoseConcept##T; #define GTSAM_CONCEPT_POSE_TYPE(T) using _gtsam_PoseConcept##T = gtsam::PoseConcept<T>;

3
gtsam/geometry/geometry.i
View File

@ -473,6 +473,9 @@ class Pose3 {
Vector logmap(const gtsam::Pose3& pose); Vector logmap(const gtsam::Pose3& pose);
Matrix AdjointMap() const; Matrix AdjointMap() const;
Vector Adjoint(Vector xi) const; Vector Adjoint(Vector xi) const;
Vector AdjointTranspose(Vector xi) const;
static Matrix adjointMap(Vector xi);
static Vector adjoint(Vector xi, Vector y);
static Matrix adjointMap_(Vector xi); static Matrix adjointMap_(Vector xi);
static Vector adjoint_(Vector xi, Vector y); static Vector adjoint_(Vector xi, Vector y);
static Vector adjointTranspose(Vector xi, Vector y); static Vector adjointTranspose(Vector xi, Vector y);

102
gtsam/geometry/tests/testPose3.cpp
View File

@ -145,6 +145,81 @@ TEST(Pose3, Adjoint_full)
EXPECT(assert_equal(expected3, Pose3::Expmap(xiprime3), 1e-6)); EXPECT(assert_equal(expected3, Pose3::Expmap(xiprime3), 1e-6));
} }
/* ************************************************************************* */
// Check Adjoint numerical derivatives
TEST(Pose3, Adjoint_jacobians)
{
Vector6 xi = (Vector6() << 0.1, 1.2, 2.3, 3.1, 1.4, 4.5).finished();
// Check evaluation sanity check
EQUALITY(static_cast<gtsam::Vector>(T.AdjointMap() * xi), T.Adjoint(xi));
EQUALITY(static_cast<gtsam::Vector>(T2.AdjointMap() * xi), T2.Adjoint(xi));
EQUALITY(static_cast<gtsam::Vector>(T3.AdjointMap() * xi), T3.Adjoint(xi));
// Check jacobians
Matrix6 actualH1, actualH2, expectedH1, expectedH2;
std::function<Vector6(const Pose3&, const Vector6&)> Adjoint_proxy =
[&](const Pose3& T, const Vector6& xi) { return T.Adjoint(xi); };
T.Adjoint(xi, actualH1, actualH2);
expectedH1 = numericalDerivative21(Adjoint_proxy, T, xi);
expectedH2 = numericalDerivative22(Adjoint_proxy, T, xi);
EXPECT(assert_equal(expectedH1, actualH1));
EXPECT(assert_equal(expectedH2, actualH2));
T2.Adjoint(xi, actualH1, actualH2);
expectedH1 = numericalDerivative21(Adjoint_proxy, T2, xi);
expectedH2 = numericalDerivative22(Adjoint_proxy, T2, xi);
EXPECT(assert_equal(expectedH1, actualH1));
EXPECT(assert_equal(expectedH2, actualH2));
T3.Adjoint(xi, actualH1, actualH2);
expectedH1 = numericalDerivative21(Adjoint_proxy, T3, xi);
expectedH2 = numericalDerivative22(Adjoint_proxy, T3, xi);
EXPECT(assert_equal(expectedH1, actualH1));
EXPECT(assert_equal(expectedH2, actualH2));
}
/* ************************************************************************* */
// Check AdjointTranspose and jacobians
TEST(Pose3, AdjointTranspose)
{
Vector6 xi = (Vector6() << 0.1, 1.2, 2.3, 3.1, 1.4, 4.5).finished();
// Check evaluation
EQUALITY(static_cast<Vector>(T.AdjointMap().transpose() * xi),
T.AdjointTranspose(xi));
EQUALITY(static_cast<Vector>(T2.AdjointMap().transpose() * xi),
T2.AdjointTranspose(xi));
EQUALITY(static_cast<Vector>(T3.AdjointMap().transpose() * xi),
T3.AdjointTranspose(xi));
// Check jacobians
Matrix6 actualH1, actualH2, expectedH1, expectedH2;
std::function<Vector6(const Pose3&, const Vector6&)> AdjointTranspose_proxy =
[&](const Pose3& T, const Vector6& xi) {
return T.AdjointTranspose(xi);
};
T.AdjointTranspose(xi, actualH1, actualH2);
expectedH1 = numericalDerivative21(AdjointTranspose_proxy, T, xi);
expectedH2 = numericalDerivative22(AdjointTranspose_proxy, T, xi);
EXPECT(assert_equal(expectedH1, actualH1, 1e-8));
EXPECT(assert_equal(expectedH2, actualH2));
T2.AdjointTranspose(xi, actualH1, actualH2);
expectedH1 = numericalDerivative21(AdjointTranspose_proxy, T2, xi);
expectedH2 = numericalDerivative22(AdjointTranspose_proxy, T2, xi);
EXPECT(assert_equal(expectedH1, actualH1, 1e-8));
EXPECT(assert_equal(expectedH2, actualH2));
T3.AdjointTranspose(xi, actualH1, actualH2);
expectedH1 = numericalDerivative21(AdjointTranspose_proxy, T3, xi);
expectedH2 = numericalDerivative22(AdjointTranspose_proxy, T3, xi);
EXPECT(assert_equal(expectedH1, actualH1, 1e-8));
EXPECT(assert_equal(expectedH2, actualH2));
}
/* ************************************************************************* */ /* ************************************************************************* */
// assert that T*wedge(xi)*T^-1 is equal to wedge(Ad_T(xi)) // assert that T*wedge(xi)*T^-1 is equal to wedge(Ad_T(xi))
TEST(Pose3, Adjoint_hat) TEST(Pose3, Adjoint_hat)
@ -837,16 +912,20 @@ Vector6 testDerivAdjoint(const Vector6& xi, const Vector6& v) {
} }
TEST( Pose3, adjoint) { TEST( Pose3, adjoint) {
Vector expected = testDerivAdjoint(screwPose3::xi, screwPose3::xi); Vector6 v = (Vector6() << 1, 2, 3, 4, 5, 6).finished();
Vector expected = testDerivAdjoint(screwPose3::xi, v);
Matrix actualH; Matrix actualH1, actualH2;
Vector actual = Pose3::adjoint(screwPose3::xi, screwPose3::xi, actualH); Vector actual = Pose3::adjoint(screwPose3::xi, v, actualH1, actualH2);
Matrix numericalH = numericalDerivative21<Vector6, Vector6, Vector6>( Matrix numericalH1 = numericalDerivative21<Vector6, Vector6, Vector6>(
testDerivAdjoint, screwPose3::xi, screwPose3::xi, 1e-5); testDerivAdjoint, screwPose3::xi, v, 1e-5);
Matrix numericalH2 = numericalDerivative22<Vector6, Vector6, Vector6>(
testDerivAdjoint, screwPose3::xi, v, 1e-5);
EXPECT(assert_equal(expected,actual,1e-5)); EXPECT(assert_equal(expected,actual,1e-5));
EXPECT(assert_equal(numericalH,actualH,1e-5)); EXPECT(assert_equal(numericalH1,actualH1,1e-5));
EXPECT(assert_equal(numericalH2,actualH2,1e-5));
} }
/* ************************************************************************* */ /* ************************************************************************* */
@ -859,14 +938,17 @@ TEST( Pose3, adjointTranspose) {
Vector v = (Vector(6) << 0.04, 0.05, 0.06, 4.0, 5.0, 6.0).finished(); Vector v = (Vector(6) << 0.04, 0.05, 0.06, 4.0, 5.0, 6.0).finished();
Vector expected = testDerivAdjointTranspose(xi, v); Vector expected = testDerivAdjointTranspose(xi, v);
Matrix actualH; Matrix actualH1, actualH2;
Vector actual = Pose3::adjointTranspose(xi, v, actualH); Vector actual = Pose3::adjointTranspose(xi, v, actualH1, actualH2);
Matrix numericalH = numericalDerivative21<Vector6, Vector6, Vector6>( Matrix numericalH1 = numericalDerivative21<Vector6, Vector6, Vector6>(
testDerivAdjointTranspose, xi, v, 1e-5);
Matrix numericalH2 = numericalDerivative22<Vector6, Vector6, Vector6>(
testDerivAdjointTranspose, xi, v, 1e-5); testDerivAdjointTranspose, xi, v, 1e-5);
EXPECT(assert_equal(expected,actual,1e-15)); EXPECT(assert_equal(expected,actual,1e-15));
EXPECT(assert_equal(numericalH,actualH,1e-5)); EXPECT(assert_equal(numericalH1,actualH1,1e-5));
EXPECT(assert_equal(numericalH2,actualH2,1e-5));
} }
/* ************************************************************************* */ /* ************************************************************************* */

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

@ -122,6 +122,21 @@ TEST( Rot3, AxisAngle)
CHECK(assert_equal(expected,actual3,1e-5)); CHECK(assert_equal(expected,actual3,1e-5));
} }
/* ************************************************************************* */
TEST( Rot3, AxisAngle2)
{
// constructor from a rotation matrix, as doubles in *row-major* order.
Rot3 R1(-0.999957, 0.00922903, 0.00203116, 0.00926964, 0.999739, 0.0208927, -0.0018374, 0.0209105, -0.999781);
Unit3 actualAxis;
double actualAngle;
// convert Rot3 to quaternion using GTSAM
std::tie(actualAxis, actualAngle) = R1.axisAngle();
double expectedAngle = 3.1396582;
CHECK(assert_equal(expectedAngle, actualAngle, 1e-5));
}
/* ************************************************************************* */ /* ************************************************************************* */
TEST( Rot3, Rodrigues) TEST( Rot3, Rodrigues)
{ {
@ -181,13 +196,13 @@ TEST( Rot3, retract)
} }
/* ************************************************************************* */ /* ************************************************************************* */
TEST(Rot3, log) { TEST( Rot3, log) {
static const double PI = boost::math::constants::pi<double>(); static const double PI = boost::math::constants::pi<double>();
Vector w; Vector w;
Rot3 R; Rot3 R;
#define CHECK_OMEGA(X, Y, Z) \ #define CHECK_OMEGA(X, Y, Z) \
w = (Vector(3) << X, Y, Z).finished(); \ w = (Vector(3) << (X), (Y), (Z)).finished(); \
R = Rot3::Rodrigues(w); \ R = Rot3::Rodrigues(w); \
EXPECT(assert_equal(w, Rot3::Logmap(R), 1e-12)); EXPECT(assert_equal(w, Rot3::Logmap(R), 1e-12));
@ -219,17 +234,17 @@ TEST(Rot3, log) {
CHECK_OMEGA(0, 0, PI) CHECK_OMEGA(0, 0, PI)
// Windows and Linux have flipped sign in quaternion mode // Windows and Linux have flipped sign in quaternion mode
#if !defined(__APPLE__) && defined(GTSAM_USE_QUATERNIONS) //#if !defined(__APPLE__) && defined(GTSAM_USE_QUATERNIONS)
w = (Vector(3) << x * PI, y * PI, z * PI).finished(); w = (Vector(3) << x * PI, y * PI, z * PI).finished();
R = Rot3::Rodrigues(w); R = Rot3::Rodrigues(w);
EXPECT(assert_equal(Vector(-w), Rot3::Logmap(R), 1e-12)); EXPECT(assert_equal(Vector(-w), Rot3::Logmap(R), 1e-12));
#else //#else
CHECK_OMEGA(x * PI, y * PI, z * PI) // CHECK_OMEGA(x * PI, y * PI, z * PI)
#endif //#endif
// Check 360 degree rotations // Check 360 degree rotations
#define CHECK_OMEGA_ZERO(X, Y, Z) \ #define CHECK_OMEGA_ZERO(X, Y, Z) \
w = (Vector(3) << X, Y, Z).finished(); \ w = (Vector(3) << (X), (Y), (Z)).finished(); \
R = Rot3::Rodrigues(w); \ R = Rot3::Rodrigues(w); \
EXPECT(assert_equal((Vector)Z_3x1, Rot3::Logmap(R))); EXPECT(assert_equal((Vector)Z_3x1, Rot3::Logmap(R)));
@ -247,15 +262,15 @@ TEST(Rot3, log) {
// Rot3's Logmap returns different, but equivalent compacted // Rot3's Logmap returns different, but equivalent compacted
// axis-angle vectors depending on whether Rot3 is implemented // axis-angle vectors depending on whether Rot3 is implemented
// by Quaternions or SO3. // by Quaternions or SO3.
#if defined(GTSAM_USE_QUATERNIONS) #if defined(GTSAM_USE_QUATERNIONS)
// Quaternion bounds angle to [-pi, pi] resulting in ~179.9 degrees // Quaternion bounds angle to [-pi, pi] resulting in ~179.9 degrees
EXPECT(assert_equal(Vector3(0.264451979, -0.742197651, -3.04098211), EXPECT(assert_equal(Vector3(0.264451979, -0.742197651, -3.04098211),
(Vector)Rot3::Logmap(Rlund), 1e-8));
#else
// SO3 will be approximate because of the non-orthogonality
EXPECT(assert_equal(Vector3(0.264452, -0.742197708, -3.04098184),
(Vector)Rot3::Logmap(Rlund), 1e-8)); (Vector)Rot3::Logmap(Rlund), 1e-8));
#else #endif
// SO3 does not bound angle resulting in ~180.1 degrees
EXPECT(assert_equal(Vector3(-0.264544406, 0.742217405, 3.04117314),
(Vector)Rot3::Logmap(Rlund), 1e-8));
#endif
} }
/* ************************************************************************* */ /* ************************************************************************* */

163
gtsam/geometry/tests/testSphericalCamera.cpp Normal file
View File

@ -0,0 +1,163 @@
/* ----------------------------------------------------------------------------
* 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 SphericalCamera.h
* @brief Calibrated camera with spherical projection
* @date Aug 26, 2021
* @author Luca Carlone
*/
#include <CppUnitLite/TestHarness.h>
#include <gtsam/base/Testable.h>
#include <gtsam/base/numericalDerivative.h>
#include <gtsam/geometry/SphericalCamera.h>
#include <cmath>
#include <iostream>
using namespace std::placeholders;
using namespace std;
using namespace gtsam;
typedef SphericalCamera Camera;
// static const Cal3_S2 K(625, 625, 0, 0, 0);
//
static const Pose3 pose(Rot3(Vector3(1, -1, -1).asDiagonal()),
Point3(0, 0, 0.5));
static const Camera camera(pose);
//
static const Pose3 pose1(Rot3(), Point3(0, 1, 0.5));
static const Camera camera1(pose1);
static const Point3 point1(-0.08, -0.08, 0.0);
static const Point3 point2(-0.08, 0.08, 0.0);
static const Point3 point3(0.08, 0.08, 0.0);
static const Point3 point4(0.08, -0.08, 0.0);
// manually computed in matlab
static const Unit3 bearing1(-0.156054862928174, 0.156054862928174,
0.975342893301088);
static const Unit3 bearing2(-0.156054862928174, -0.156054862928174,
0.975342893301088);
static const Unit3 bearing3(0.156054862928174, -0.156054862928174,
0.975342893301088);
static const Unit3 bearing4(0.156054862928174, 0.156054862928174,
0.975342893301088);
static double depth = 0.512640224719052;
/* ************************************************************************* */
TEST(SphericalCamera, constructor) {
EXPECT(assert_equal(pose, camera.pose()));
}
/* ************************************************************************* */
TEST(SphericalCamera, project) {
// expected from manual calculation in Matlab
EXPECT(assert_equal(camera.project(point1), bearing1));
EXPECT(assert_equal(camera.project(point2), bearing2));
EXPECT(assert_equal(camera.project(point3), bearing3));
EXPECT(assert_equal(camera.project(point4), bearing4));
}
/* ************************************************************************* */
TEST(SphericalCamera, backproject) {
EXPECT(assert_equal(camera.backproject(bearing1, depth), point1));
EXPECT(assert_equal(camera.backproject(bearing2, depth), point2));
EXPECT(assert_equal(camera.backproject(bearing3, depth), point3));
EXPECT(assert_equal(camera.backproject(bearing4, depth), point4));
}
/* ************************************************************************* */
TEST(SphericalCamera, backproject2) {
Point3 origin(0, 0, 0);
Rot3 rot(1., 0., 0., 0., 0., 1., 0., -1., 0.); // a camera1 looking down
Camera camera(Pose3(rot, origin));
Point3 actual = camera.backproject(Unit3(0, 0, 1), 1.);
Point3 expected(0., 1., 0.);
pair<Unit3, bool> x = camera.projectSafe(expected);
EXPECT(assert_equal(expected, actual));
EXPECT(assert_equal(Unit3(0, 0, 1), x.first));
EXPECT(x.second);
}
/* ************************************************************************* */
static Unit3 project3(const Pose3& pose, const Point3& point) {
return Camera(pose).project(point);
}
/* ************************************************************************* */
TEST(SphericalCamera, Dproject) {
Matrix Dpose, Dpoint;
Unit3 result = camera.project(point1, Dpose, Dpoint);
Matrix numerical_pose = numericalDerivative21(project3, pose, point1);
Matrix numerical_point = numericalDerivative22(project3, pose, point1);
EXPECT(assert_equal(bearing1, result));
EXPECT(assert_equal(numerical_pose, Dpose, 1e-7));
EXPECT(assert_equal(numerical_point, Dpoint, 1e-7));
}
/* ************************************************************************* */
static Vector2 reprojectionError2(const Pose3& pose, const Point3& point,
const Unit3& measured) {
return Camera(pose).reprojectionError(point, measured);
}
/* ************************************************************************* */
TEST(SphericalCamera, reprojectionError) {
Matrix Dpose, Dpoint;
Vector2 result = camera.reprojectionError(point1, bearing1, Dpose, Dpoint);
Matrix numerical_pose =
numericalDerivative31(reprojectionError2, pose, point1, bearing1);
Matrix numerical_point =
numericalDerivative32(reprojectionError2, pose, point1, bearing1);
EXPECT(assert_equal(Vector2(0.0, 0.0), result));
EXPECT(assert_equal(numerical_pose, Dpose, 1e-7));
EXPECT(assert_equal(numerical_point, Dpoint, 1e-7));
}
/* ************************************************************************* */
TEST(SphericalCamera, reprojectionError_noisy) {
Matrix Dpose, Dpoint;
Unit3 bearing_noisy = bearing1.retract(Vector2(0.01, 0.05));
Vector2 result =
camera.reprojectionError(point1, bearing_noisy, Dpose, Dpoint);
Matrix numerical_pose =
numericalDerivative31(reprojectionError2, pose, point1, bearing_noisy);
Matrix numerical_point =
numericalDerivative32(reprojectionError2, pose, point1, bearing_noisy);
EXPECT(assert_equal(Vector2(-0.050282, 0.00833482), result, 1e-5));
EXPECT(assert_equal(numerical_pose, Dpose, 1e-7));
EXPECT(assert_equal(numerical_point, Dpoint, 1e-7));
}
/* ************************************************************************* */
// Add a test with more arbitrary rotation
TEST(SphericalCamera, Dproject2) {
static const Pose3 pose1(Rot3::Ypr(0.1, -0.1, 0.4), Point3(0, 0, -10));
static const Camera camera(pose1);
Matrix Dpose, Dpoint;
camera.project2(point1, Dpose, Dpoint);
Matrix numerical_pose = numericalDerivative21(project3, pose1, point1);
Matrix numerical_point = numericalDerivative22(project3, pose1, point1);
CHECK(assert_equal(numerical_pose, Dpose, 1e-7));
CHECK(assert_equal(numerical_point, Dpoint, 1e-7));
}
/* ************************************************************************* */
int main() {
TestResult tr;
return TestRegistry::runAllTests(tr);
}
/* ************************************************************************* */

392
gtsam/geometry/tests/testTriangulation.cpp
View File

@ -10,22 +10,23 @@
* -------------------------------------------------------------------------- */ * -------------------------------------------------------------------------- */
/** /**
* testTriangulation.cpp * @file testTriangulation.cpp
* * @brief triangulation utilities
* Created on: July 30th, 2013 * @date July 30th, 2013
* Author: cbeall3 * @author Chris Beall (cbeall3)
* @author Luca Carlone
*/ */
#include <gtsam/geometry/triangulation.h>
#include <gtsam/geometry/PinholeCamera.h>
#include <gtsam/geometry/StereoCamera.h>
#include <gtsam/geometry/CameraSet.h>
#include <gtsam/geometry/Cal3Bundler.h>
#include <gtsam/slam/StereoFactor.h>
#include <gtsam/nonlinear/LevenbergMarquardtOptimizer.h>
#include <gtsam/nonlinear/ExpressionFactor.h>
#include <CppUnitLite/TestHarness.h> #include <CppUnitLite/TestHarness.h>
#include <gtsam/geometry/Cal3Bundler.h>
#include <gtsam/geometry/CameraSet.h>
#include <gtsam/geometry/PinholeCamera.h>
#include <gtsam/geometry/SphericalCamera.h>
#include <gtsam/geometry/StereoCamera.h>
#include <gtsam/geometry/triangulation.h>
#include <gtsam/nonlinear/ExpressionFactor.h>
#include <gtsam/nonlinear/LevenbergMarquardtOptimizer.h>
#include <gtsam/slam/StereoFactor.h>
#include <boost/assign.hpp> #include <boost/assign.hpp>
#include <boost/assign/std/vector.hpp> #include <boost/assign/std/vector.hpp>
@ -36,7 +37,7 @@ using namespace boost::assign;
// Some common constants // Some common constants
static const boost::shared_ptr<Cal3_S2> sharedCal = // static const boost::shared_ptr<Cal3_S2> sharedCal = //
boost::make_shared<Cal3_S2>(1500, 1200, 0, 640, 480); boost::make_shared<Cal3_S2>(1500, 1200, 0, 640, 480);
// Looking along X-axis, 1 meter above ground plane (x-y) // Looking along X-axis, 1 meter above ground plane (x-y)
@ -57,8 +58,7 @@ Point2 z2 = camera2.project(landmark);
//****************************************************************************** //******************************************************************************
// Simple test with a well-behaved two camera situation // Simple test with a well-behaved two camera situation
TEST( triangulation, twoPoses) { TEST(triangulation, twoPoses) {
vector<Pose3> poses; vector<Pose3> poses;
Point2Vector measurements; Point2Vector measurements;
@ -69,36 +69,36 @@ TEST( triangulation, twoPoses) {
// 1. Test simple DLT, perfect in no noise situation // 1. Test simple DLT, perfect in no noise situation
bool optimize = false; bool optimize = false;
boost::optional<Point3> actual1 = // boost::optional<Point3> actual1 = //
triangulatePoint3(poses, sharedCal, measurements, rank_tol, optimize); triangulatePoint3<Cal3_S2>(poses, sharedCal, measurements, rank_tol, optimize);
EXPECT(assert_equal(landmark, *actual1, 1e-7)); EXPECT(assert_equal(landmark, *actual1, 1e-7));
// 2. test with optimization on, same answer // 2. test with optimization on, same answer
optimize = true; optimize = true;
boost::optional<Point3> actual2 = // boost::optional<Point3> actual2 = //
triangulatePoint3(poses, sharedCal, measurements, rank_tol, optimize); triangulatePoint3<Cal3_S2>(poses, sharedCal, measurements, rank_tol, optimize);
EXPECT(assert_equal(landmark, *actual2, 1e-7)); EXPECT(assert_equal(landmark, *actual2, 1e-7));
// 3. Add some noise and try again: result should be ~ (4.995, 0.499167, 1.19814) // 3. Add some noise and try again: result should be ~ (4.995,
// 0.499167, 1.19814)
measurements.at(0) += Point2(0.1, 0.5); measurements.at(0) += Point2(0.1, 0.5);
measurements.at(1) += Point2(-0.2, 0.3); measurements.at(1) += Point2(-0.2, 0.3);
optimize = false; optimize = false;
boost::optional<Point3> actual3 = // boost::optional<Point3> actual3 = //
triangulatePoint3(poses, sharedCal, measurements, rank_tol, optimize); triangulatePoint3<Cal3_S2>(poses, sharedCal, measurements, rank_tol, optimize);
EXPECT(assert_equal(Point3(4.995, 0.499167, 1.19814), *actual3, 1e-4)); EXPECT(assert_equal(Point3(4.995, 0.499167, 1.19814), *actual3, 1e-4));
// 4. Now with optimization on // 4. Now with optimization on
optimize = true; optimize = true;
boost::optional<Point3> actual4 = // boost::optional<Point3> actual4 = //
triangulatePoint3(poses, sharedCal, measurements, rank_tol, optimize); triangulatePoint3<Cal3_S2>(poses, sharedCal, measurements, rank_tol, optimize);
EXPECT(assert_equal(Point3(4.995, 0.499167, 1.19814), *actual4, 1e-4)); EXPECT(assert_equal(Point3(4.995, 0.499167, 1.19814), *actual4, 1e-4));
} }
//****************************************************************************** //******************************************************************************
// Similar, but now with Bundler calibration // Similar, but now with Bundler calibration
TEST( triangulation, twoPosesBundler) { TEST(triangulation, twoPosesBundler) {
boost::shared_ptr<Cal3Bundler> bundlerCal = //
boost::shared_ptr<Cal3Bundler> bundlerCal = //
boost::make_shared<Cal3Bundler>(1500, 0, 0, 640, 480); boost::make_shared<Cal3Bundler>(1500, 0, 0, 640, 480);
PinholeCamera<Cal3Bundler> camera1(pose1, *bundlerCal); PinholeCamera<Cal3Bundler> camera1(pose1, *bundlerCal);
PinholeCamera<Cal3Bundler> camera2(pose2, *bundlerCal); PinholeCamera<Cal3Bundler> camera2(pose2, *bundlerCal);
@ -116,37 +116,38 @@ TEST( triangulation, twoPosesBundler) {
bool optimize = true; bool optimize = true;
double rank_tol = 1e-9; double rank_tol = 1e-9;
boost::optional<Point3> actual = // boost::optional<Point3> actual = //
triangulatePoint3(poses, bundlerCal, measurements, rank_tol, optimize); triangulatePoint3<Cal3Bundler>(poses, bundlerCal, measurements, rank_tol, optimize);
EXPECT(assert_equal(landmark, *actual, 1e-7)); EXPECT(assert_equal(landmark, *actual, 1e-7));
// Add some noise and try again // Add some noise and try again
measurements.at(0) += Point2(0.1, 0.5); measurements.at(0) += Point2(0.1, 0.5);
measurements.at(1) += Point2(-0.2, 0.3); measurements.at(1) += Point2(-0.2, 0.3);
boost::optional<Point3> actual2 = // boost::optional<Point3> actual2 = //
triangulatePoint3(poses, bundlerCal, measurements, rank_tol, optimize); triangulatePoint3<Cal3Bundler>(poses, bundlerCal, measurements, rank_tol, optimize);
EXPECT(assert_equal(Point3(4.995, 0.499167, 1.19847), *actual2, 1e-4)); EXPECT(assert_equal(Point3(4.995, 0.499167, 1.19847), *actual2, 1e-4));
} }
//****************************************************************************** //******************************************************************************
TEST( triangulation, fourPoses) { TEST(triangulation, fourPoses) {
vector<Pose3> poses; vector<Pose3> poses;
Point2Vector measurements; Point2Vector measurements;
poses += pose1, pose2; poses += pose1, pose2;
measurements += z1, z2; measurements += z1, z2;
boost::optional<Point3> actual = triangulatePoint3(poses, sharedCal, boost::optional<Point3> actual =
measurements); triangulatePoint3<Cal3_S2>(poses, sharedCal, measurements);
EXPECT(assert_equal(landmark, *actual, 1e-2)); EXPECT(assert_equal(landmark, *actual, 1e-2));
// 2. Add some noise and try again: result should be ~ (4.995, 0.499167, 1.19814) // 2. Add some noise and try again: result should be ~ (4.995,
// 0.499167, 1.19814)
measurements.at(0) += Point2(0.1, 0.5); measurements.at(0) += Point2(0.1, 0.5);
measurements.at(1) += Point2(-0.2, 0.3); measurements.at(1) += Point2(-0.2, 0.3);
boost::optional<Point3> actual2 = // boost::optional<Point3> actual2 = //
triangulatePoint3(poses, sharedCal, measurements); triangulatePoint3<Cal3_S2>(poses, sharedCal, measurements);
EXPECT(assert_equal(landmark, *actual2, 1e-2)); EXPECT(assert_equal(landmark, *actual2, 1e-2));
// 3. Add a slightly rotated third camera above, again with measurement noise // 3. Add a slightly rotated third camera above, again with measurement noise
@ -157,13 +158,13 @@ TEST( triangulation, fourPoses) {
poses += pose3; poses += pose3;
measurements += z3 + Point2(0.1, -0.1); measurements += z3 + Point2(0.1, -0.1);
boost::optional<Point3> triangulated_3cameras = // boost::optional<Point3> triangulated_3cameras = //
triangulatePoint3(poses, sharedCal, measurements); triangulatePoint3<Cal3_S2>(poses, sharedCal, measurements);
EXPECT(assert_equal(landmark, *triangulated_3cameras, 1e-2)); EXPECT(assert_equal(landmark, *triangulated_3cameras, 1e-2));
// Again with nonlinear optimization // Again with nonlinear optimization
boost::optional<Point3> triangulated_3cameras_opt = triangulatePoint3(poses, boost::optional<Point3> triangulated_3cameras_opt =
sharedCal, measurements, 1e-9, true); triangulatePoint3<Cal3_S2>(poses, sharedCal, measurements, 1e-9, true);
EXPECT(assert_equal(landmark, *triangulated_3cameras_opt, 1e-2)); EXPECT(assert_equal(landmark, *triangulated_3cameras_opt, 1e-2));
// 4. Test failure: Add a 4th camera facing the wrong way // 4. Test failure: Add a 4th camera facing the wrong way
@ -176,13 +177,13 @@ TEST( triangulation, fourPoses) {
poses += pose4; poses += pose4;
measurements += Point2(400, 400); measurements += Point2(400, 400);
CHECK_EXCEPTION(triangulatePoint3(poses, sharedCal, measurements), CHECK_EXCEPTION(triangulatePoint3<Cal3_S2>(poses, sharedCal, measurements),
TriangulationCheiralityException); TriangulationCheiralityException);
#endif #endif
} }
//****************************************************************************** //******************************************************************************
TEST( triangulation, fourPoses_distinct_Ks) { TEST(triangulation, fourPoses_distinct_Ks) {
Cal3_S2 K1(1500, 1200, 0, 640, 480); Cal3_S2 K1(1500, 1200, 0, 640, 480);
// create first camera. Looking along X-axis, 1 meter above ground plane (x-y) // create first camera. Looking along X-axis, 1 meter above ground plane (x-y)
PinholeCamera<Cal3_S2> camera1(pose1, K1); PinholeCamera<Cal3_S2> camera1(pose1, K1);
@ -195,22 +196,23 @@ TEST( triangulation, fourPoses_distinct_Ks) {
Point2 z1 = camera1.project(landmark); Point2 z1 = camera1.project(landmark);
Point2 z2 = camera2.project(landmark); Point2 z2 = camera2.project(landmark);
CameraSet<PinholeCamera<Cal3_S2> > cameras; CameraSet<PinholeCamera<Cal3_S2>> cameras;
Point2Vector measurements; Point2Vector measurements;
cameras += camera1, camera2; cameras += camera1, camera2;
measurements += z1, z2; measurements += z1, z2;
boost::optional<Point3> actual = // boost::optional<Point3> actual = //
triangulatePoint3(cameras, measurements); triangulatePoint3<Cal3_S2>(cameras, measurements);
EXPECT(assert_equal(landmark, *actual, 1e-2)); EXPECT(assert_equal(landmark, *actual, 1e-2));
// 2. Add some noise and try again: result should be ~ (4.995, 0.499167, 1.19814) // 2. Add some noise and try again: result should be ~ (4.995,
// 0.499167, 1.19814)
measurements.at(0) += Point2(0.1, 0.5); measurements.at(0) += Point2(0.1, 0.5);
measurements.at(1) += Point2(-0.2, 0.3); measurements.at(1) += Point2(-0.2, 0.3);
boost::optional<Point3> actual2 = // boost::optional<Point3> actual2 = //
triangulatePoint3(cameras, measurements); triangulatePoint3<Cal3_S2>(cameras, measurements);
EXPECT(assert_equal(landmark, *actual2, 1e-2)); EXPECT(assert_equal(landmark, *actual2, 1e-2));
// 3. Add a slightly rotated third camera above, again with measurement noise // 3. Add a slightly rotated third camera above, again with measurement noise
@ -222,13 +224,13 @@ TEST( triangulation, fourPoses_distinct_Ks) {
cameras += camera3; cameras += camera3;
measurements += z3 + Point2(0.1, -0.1); measurements += z3 + Point2(0.1, -0.1);
boost::optional<Point3> triangulated_3cameras = // boost::optional<Point3> triangulated_3cameras = //
triangulatePoint3(cameras, measurements); triangulatePoint3<Cal3_S2>(cameras, measurements);
EXPECT(assert_equal(landmark, *triangulated_3cameras, 1e-2)); EXPECT(assert_equal(landmark, *triangulated_3cameras, 1e-2));
// Again with nonlinear optimization // Again with nonlinear optimization
boost::optional<Point3> triangulated_3cameras_opt = triangulatePoint3(cameras, boost::optional<Point3> triangulated_3cameras_opt =
measurements, 1e-9, true); triangulatePoint3<Cal3_S2>(cameras, measurements, 1e-9, true);
EXPECT(assert_equal(landmark, *triangulated_3cameras_opt, 1e-2)); EXPECT(assert_equal(landmark, *triangulated_3cameras_opt, 1e-2));
// 4. Test failure: Add a 4th camera facing the wrong way // 4. Test failure: Add a 4th camera facing the wrong way
@ -241,13 +243,13 @@ TEST( triangulation, fourPoses_distinct_Ks) {
cameras += camera4; cameras += camera4;
measurements += Point2(400, 400); measurements += Point2(400, 400);
CHECK_EXCEPTION(triangulatePoint3(cameras, measurements), CHECK_EXCEPTION(triangulatePoint3<Cal3_S2>(cameras, measurements),
TriangulationCheiralityException); TriangulationCheiralityException);
#endif #endif
} }
//****************************************************************************** //******************************************************************************
TEST( triangulation, outliersAndFarLandmarks) { TEST(triangulation, outliersAndFarLandmarks) {
Cal3_S2 K1(1500, 1200, 0, 640, 480); Cal3_S2 K1(1500, 1200, 0, 640, 480);
// create first camera. Looking along X-axis, 1 meter above ground plane (x-y) // create first camera. Looking along X-axis, 1 meter above ground plane (x-y)
PinholeCamera<Cal3_S2> camera1(pose1, K1); PinholeCamera<Cal3_S2> camera1(pose1, K1);
@ -260,24 +262,29 @@ TEST( triangulation, outliersAndFarLandmarks) {
Point2 z1 = camera1.project(landmark); Point2 z1 = camera1.project(landmark);
Point2 z2 = camera2.project(landmark); Point2 z2 = camera2.project(landmark);
CameraSet<PinholeCamera<Cal3_S2> > cameras; CameraSet<PinholeCamera<Cal3_S2>> cameras;
Point2Vector measurements; Point2Vector measurements;
cameras += camera1, camera2; cameras += camera1, camera2;
measurements += z1, z2; measurements += z1, z2;
double landmarkDistanceThreshold = 10; // landmark is closer than that double landmarkDistanceThreshold = 10; // landmark is closer than that
TriangulationParameters params(1.0, false, landmarkDistanceThreshold); // all default except landmarkDistanceThreshold TriangulationParameters params(
TriangulationResult actual = triangulateSafe(cameras,measurements,params); 1.0, false, landmarkDistanceThreshold); // all default except
// landmarkDistanceThreshold
TriangulationResult actual = triangulateSafe(cameras, measurements, params);
EXPECT(assert_equal(landmark, *actual, 1e-2)); EXPECT(assert_equal(landmark, *actual, 1e-2));
EXPECT(actual.valid()); EXPECT(actual.valid());
landmarkDistanceThreshold = 4; // landmark is farther than that landmarkDistanceThreshold = 4; // landmark is farther than that
TriangulationParameters params2(1.0, false, landmarkDistanceThreshold); // all default except landmarkDistanceThreshold TriangulationParameters params2(
actual = triangulateSafe(cameras,measurements,params2); 1.0, false, landmarkDistanceThreshold); // all default except
// landmarkDistanceThreshold
actual = triangulateSafe(cameras, measurements, params2);
EXPECT(actual.farPoint()); EXPECT(actual.farPoint());
// 3. Add a slightly rotated third camera above with a wrong measurement (OUTLIER) // 3. Add a slightly rotated third camera above with a wrong measurement
// (OUTLIER)
Pose3 pose3 = pose1 * Pose3(Rot3::Ypr(0.1, 0.2, 0.1), Point3(0.1, -2, -.1)); Pose3 pose3 = pose1 * Pose3(Rot3::Ypr(0.1, 0.2, 0.1), Point3(0.1, -2, -.1));
Cal3_S2 K3(700, 500, 0, 640, 480); Cal3_S2 K3(700, 500, 0, 640, 480);
PinholeCamera<Cal3_S2> camera3(pose3, K3); PinholeCamera<Cal3_S2> camera3(pose3, K3);
@ -286,21 +293,23 @@ TEST( triangulation, outliersAndFarLandmarks) {
cameras += camera3; cameras += camera3;
measurements += z3 + Point2(10, -10); measurements += z3 + Point2(10, -10);
landmarkDistanceThreshold = 10; // landmark is closer than that landmarkDistanceThreshold = 10; // landmark is closer than that
double outlierThreshold = 100; // loose, the outlier is going to pass double outlierThreshold = 100; // loose, the outlier is going to pass
TriangulationParameters params3(1.0, false, landmarkDistanceThreshold,outlierThreshold); TriangulationParameters params3(1.0, false, landmarkDistanceThreshold,
actual = triangulateSafe(cameras,measurements,params3); outlierThreshold);
actual = triangulateSafe(cameras, measurements, params3);
EXPECT(actual.valid()); EXPECT(actual.valid());
// now set stricter threshold for outlier rejection // now set stricter threshold for outlier rejection
outlierThreshold = 5; // tighter, the outlier is not going to pass outlierThreshold = 5; // tighter, the outlier is not going to pass
TriangulationParameters params4(1.0, false, landmarkDistanceThreshold,outlierThreshold); TriangulationParameters params4(1.0, false, landmarkDistanceThreshold,
actual = triangulateSafe(cameras,measurements,params4); outlierThreshold);
actual = triangulateSafe(cameras, measurements, params4);
EXPECT(actual.outlier()); EXPECT(actual.outlier());
} }
//****************************************************************************** //******************************************************************************
TEST( triangulation, twoIdenticalPoses) { TEST(triangulation, twoIdenticalPoses) {
// create first camera. Looking along X-axis, 1 meter above ground plane (x-y) // create first camera. Looking along X-axis, 1 meter above ground plane (x-y)
PinholeCamera<Cal3_S2> camera1(pose1, *sharedCal); PinholeCamera<Cal3_S2> camera1(pose1, *sharedCal);
@ -313,12 +322,12 @@ TEST( triangulation, twoIdenticalPoses) {
poses += pose1, pose1; poses += pose1, pose1;
measurements += z1, z1; measurements += z1, z1;
CHECK_EXCEPTION(triangulatePoint3(poses, sharedCal, measurements), CHECK_EXCEPTION(triangulatePoint3<Cal3_S2>(poses, sharedCal, measurements),
TriangulationUnderconstrainedException); TriangulationUnderconstrainedException);
} }
//****************************************************************************** //******************************************************************************
TEST( triangulation, onePose) { TEST(triangulation, onePose) {
// we expect this test to fail with a TriangulationUnderconstrainedException // we expect this test to fail with a TriangulationUnderconstrainedException
// because there's only one camera observation // because there's only one camera observation
@ -326,28 +335,26 @@ TEST( triangulation, onePose) {
Point2Vector measurements; Point2Vector measurements;
poses += Pose3(); poses += Pose3();
measurements += Point2(0,0); measurements += Point2(0, 0);
CHECK_EXCEPTION(triangulatePoint3(poses, sharedCal, measurements), CHECK_EXCEPTION(triangulatePoint3<Cal3_S2>(poses, sharedCal, measurements),
TriangulationUnderconstrainedException); TriangulationUnderconstrainedException);
} }
//****************************************************************************** //******************************************************************************
TEST( triangulation, StereotriangulateNonlinear ) { TEST(triangulation, StereotriangulateNonlinear) {
auto stereoK = boost::make_shared<Cal3_S2Stereo>(1733.75, 1733.75, 0, 689.645,
auto stereoK = boost::make_shared<Cal3_S2Stereo>(1733.75, 1733.75, 0, 689.645, 508.835, 0.0699612); 508.835, 0.0699612);
// two camera poses m1, m2 // two camera poses m1, m2
Matrix4 m1, m2; Matrix4 m1, m2;
m1 << 0.796888717, 0.603404026, -0.0295271487, 46.6673779, m1 << 0.796888717, 0.603404026, -0.0295271487, 46.6673779, 0.592783835,
0.592783835, -0.77156583, 0.230856632, 66.2186159, -0.77156583, 0.230856632, 66.2186159, 0.116517574, -0.201470143,
0.116517574, -0.201470143, -0.9725393, -4.28382528, -0.9725393, -4.28382528, 0, 0, 0, 1;
0, 0, 0, 1;
m2 << -0.955959025, -0.29288915, -0.0189328569, 45.7169799, m2 << -0.955959025, -0.29288915, -0.0189328569, 45.7169799, -0.29277519,
-0.29277519, 0.947083213, 0.131587097, 65.843136, 0.947083213, 0.131587097, 65.843136, -0.0206094928, 0.131334858,
-0.0206094928, 0.131334858, -0.991123524, -4.3525033, -0.991123524, -4.3525033, 0, 0, 0, 1;
0, 0, 0, 1;
typedef CameraSet<StereoCamera> Cameras; typedef CameraSet<StereoCamera> Cameras;
Cameras cameras; Cameras cameras;
@ -358,18 +365,19 @@ TEST( triangulation, StereotriangulateNonlinear ) {
measurements += StereoPoint2(226.936, 175.212, 424.469); measurements += StereoPoint2(226.936, 175.212, 424.469);
measurements += StereoPoint2(339.571, 285.547, 669.973); measurements += StereoPoint2(339.571, 285.547, 669.973);
Point3 initial = Point3(46.0536958, 66.4621179, -6.56285929); // error: 96.5715555191 Point3 initial =
Point3(46.0536958, 66.4621179, -6.56285929); // error: 96.5715555191
Point3 actual = triangulateNonlinear(cameras, measurements, initial); Point3 actual = triangulateNonlinear<StereoCamera>(cameras, measurements, initial);
Point3 expected(46.0484569, 66.4710686, -6.55046613); // error: 0.763510644187 Point3 expected(46.0484569, 66.4710686,
-6.55046613); // error: 0.763510644187
EXPECT(assert_equal(expected, actual, 1e-4)); EXPECT(assert_equal(expected, actual, 1e-4));
// regular stereo factor comparison - expect very similar result as above // regular stereo factor comparison - expect very similar result as above
{ {
typedef GenericStereoFactor<Pose3,Point3> StereoFactor; typedef GenericStereoFactor<Pose3, Point3> StereoFactor;
Values values; Values values;
values.insert(Symbol('x', 1), Pose3(m1)); values.insert(Symbol('x', 1), Pose3(m1));
@ -378,17 +386,19 @@ TEST( triangulation, StereotriangulateNonlinear ) {
NonlinearFactorGraph graph; NonlinearFactorGraph graph;
static SharedNoiseModel unit(noiseModel::Unit::Create(3)); static SharedNoiseModel unit(noiseModel::Unit::Create(3));
graph.emplace_shared<StereoFactor>(measurements[0], unit, Symbol('x',1), Symbol('l',1), stereoK); graph.emplace_shared<StereoFactor>(measurements[0], unit, Symbol('x', 1),
graph.emplace_shared<StereoFactor>(measurements[1], unit, Symbol('x',2), Symbol('l',1), stereoK); Symbol('l', 1), stereoK);
graph.emplace_shared<StereoFactor>(measurements[1], unit, Symbol('x', 2),
Symbol('l', 1), stereoK);
const SharedDiagonal posePrior = noiseModel::Isotropic::Sigma(6, 1e-9); const SharedDiagonal posePrior = noiseModel::Isotropic::Sigma(6, 1e-9);
graph.addPrior(Symbol('x',1), Pose3(m1), posePrior); graph.addPrior(Symbol('x', 1), Pose3(m1), posePrior);
graph.addPrior(Symbol('x',2), Pose3(m2), posePrior); graph.addPrior(Symbol('x', 2), Pose3(m2), posePrior);
LevenbergMarquardtOptimizer optimizer(graph, values); LevenbergMarquardtOptimizer optimizer(graph, values);
Values result = optimizer.optimize(); Values result = optimizer.optimize();
EXPECT(assert_equal(expected, result.at<Point3>(Symbol('l',1)), 1e-4)); EXPECT(assert_equal(expected, result.at<Point3>(Symbol('l', 1)), 1e-4));
} }
// use Triangulation Factor directly - expect same result as above // use Triangulation Factor directly - expect same result as above
@ -399,13 +409,15 @@ TEST( triangulation, StereotriangulateNonlinear ) {
NonlinearFactorGraph graph; NonlinearFactorGraph graph;
static SharedNoiseModel unit(noiseModel::Unit::Create(3)); static SharedNoiseModel unit(noiseModel::Unit::Create(3));
graph.emplace_shared<TriangulationFactor<StereoCamera> >(cameras[0], measurements[0], unit, Symbol('l',1)); graph.emplace_shared<TriangulationFactor<StereoCamera>>(
graph.emplace_shared<TriangulationFactor<StereoCamera> >(cameras[1], measurements[1], unit, Symbol('l',1)); cameras[0], measurements[0], unit, Symbol('l', 1));
graph.emplace_shared<TriangulationFactor<StereoCamera>>(
cameras[1], measurements[1], unit, Symbol('l', 1));
LevenbergMarquardtOptimizer optimizer(graph, values); LevenbergMarquardtOptimizer optimizer(graph, values);
Values result = optimizer.optimize(); Values result = optimizer.optimize();
EXPECT(assert_equal(expected, result.at<Point3>(Symbol('l',1)), 1e-4)); EXPECT(assert_equal(expected, result.at<Point3>(Symbol('l', 1)), 1e-4));
} }
// use ExpressionFactor - expect same result as above // use ExpressionFactor - expect same result as above
@ -416,11 +428,13 @@ TEST( triangulation, StereotriangulateNonlinear ) {
NonlinearFactorGraph graph; NonlinearFactorGraph graph;
static SharedNoiseModel unit(noiseModel::Unit::Create(3)); static SharedNoiseModel unit(noiseModel::Unit::Create(3));
Expression<Point3> point_(Symbol('l',1)); Expression<Point3> point_(Symbol('l', 1));
Expression<StereoCamera> camera0_(cameras[0]); Expression<StereoCamera> camera0_(cameras[0]);
Expression<StereoCamera> camera1_(cameras[1]); Expression<StereoCamera> camera1_(cameras[1]);
Expression<StereoPoint2> project0_(camera0_, &StereoCamera::project2, point_); Expression<StereoPoint2> project0_(camera0_, &StereoCamera::project2,
Expression<StereoPoint2> project1_(camera1_, &StereoCamera::project2, point_); point_);
Expression<StereoPoint2> project1_(camera1_, &StereoCamera::project2,
point_);
graph.addExpressionFactor(unit, measurements[0], project0_); graph.addExpressionFactor(unit, measurements[0], project0_);
graph.addExpressionFactor(unit, measurements[1], project1_); graph.addExpressionFactor(unit, measurements[1], project1_);
@ -428,10 +442,172 @@ TEST( triangulation, StereotriangulateNonlinear ) {
LevenbergMarquardtOptimizer optimizer(graph, values); LevenbergMarquardtOptimizer optimizer(graph, values);
Values result = optimizer.optimize(); Values result = optimizer.optimize();
EXPECT(assert_equal(expected, result.at<Point3>(Symbol('l',1)), 1e-4)); EXPECT(assert_equal(expected, result.at<Point3>(Symbol('l', 1)), 1e-4));
} }
} }
//******************************************************************************
// Simple test with a well-behaved two camera situation
TEST(triangulation, twoPoses_sphericalCamera) {
vector<Pose3> poses;
std::vector<Unit3> measurements;
// Project landmark into two cameras and triangulate
SphericalCamera cam1(pose1);
SphericalCamera cam2(pose2);
Unit3 u1 = cam1.project(landmark);
Unit3 u2 = cam2.project(landmark);
poses += pose1, pose2;
measurements += u1, u2;
CameraSet<SphericalCamera> cameras;
cameras.push_back(cam1);
cameras.push_back(cam2);
double rank_tol = 1e-9;
// 1. Test linear triangulation via DLT
auto projection_matrices = projectionMatricesFromCameras(cameras);
Point3 point = triangulateDLT(projection_matrices, measurements, rank_tol);
EXPECT(assert_equal(landmark, point, 1e-7));
// 2. Test nonlinear triangulation
point = triangulateNonlinear<SphericalCamera>(cameras, measurements, point);
EXPECT(assert_equal(landmark, point, 1e-7));
// 3. Test simple DLT, now within triangulatePoint3
bool optimize = false;
boost::optional<Point3> actual1 = //
triangulatePoint3<SphericalCamera>(cameras, measurements, rank_tol,
optimize);
EXPECT(assert_equal(landmark, *actual1, 1e-7));
// 4. test with optimization on, same answer
optimize = true;
boost::optional<Point3> actual2 = //
triangulatePoint3<SphericalCamera>(cameras, measurements, rank_tol,
optimize);
EXPECT(assert_equal(landmark, *actual2, 1e-7));
// 5. Add some noise and try again: result should be ~ (4.995,
// 0.499167, 1.19814)
measurements.at(0) =
u1.retract(Vector2(0.01, 0.05)); // note: perturbation smaller for Unit3
measurements.at(1) = u2.retract(Vector2(-0.02, 0.03));
optimize = false;
boost::optional<Point3> actual3 = //
triangulatePoint3<SphericalCamera>(cameras, measurements, rank_tol,
optimize);
EXPECT(assert_equal(Point3(5.9432, 0.654319, 1.48192), *actual3, 1e-3));
// 6. Now with optimization on
optimize = true;
boost::optional<Point3> actual4 = //
triangulatePoint3<SphericalCamera>(cameras, measurements, rank_tol,
optimize);
EXPECT(assert_equal(Point3(5.9432, 0.654334, 1.48192), *actual4, 1e-3));
}
//******************************************************************************
TEST(triangulation, twoPoses_sphericalCamera_extremeFOV) {
vector<Pose3> poses;
std::vector<Unit3> measurements;
// Project landmark into two cameras and triangulate
Pose3 poseA = Pose3(
Rot3::Ypr(-M_PI / 2, 0., -M_PI / 2),
Point3(0.0, 0.0, 0.0)); // with z pointing along x axis of global frame
Pose3 poseB = Pose3(Rot3::Ypr(-M_PI / 2, 0., -M_PI / 2),
Point3(2.0, 0.0, 0.0)); // 2m in front of poseA
Point3 landmarkL(
1.0, -1.0,
0.0); // 1m to the right of both cameras, in front of poseA, behind poseB
SphericalCamera cam1(poseA);
SphericalCamera cam2(poseB);
Unit3 u1 = cam1.project(landmarkL);
Unit3 u2 = cam2.project(landmarkL);
EXPECT(assert_equal(Unit3(Point3(1.0, 0.0, 1.0)), u1,
1e-7)); // in front and to the right of PoseA
EXPECT(assert_equal(Unit3(Point3(1.0, 0.0, -1.0)), u2,
1e-7)); // behind and to the right of PoseB
poses += pose1, pose2;
measurements += u1, u2;
CameraSet<SphericalCamera> cameras;
cameras.push_back(cam1);
cameras.push_back(cam2);
double rank_tol = 1e-9;
{
// 1. Test simple DLT, when 1 point is behind spherical camera
bool optimize = false;
#ifdef GTSAM_THROW_CHEIRALITY_EXCEPTION
CHECK_EXCEPTION(triangulatePoint3<SphericalCamera>(cameras, measurements,
rank_tol, optimize),
TriangulationCheiralityException);
#else // otherwise project should not throw the exception
boost::optional<Point3> actual1 = //
triangulatePoint3<SphericalCamera>(cameras, measurements, rank_tol,
optimize);
EXPECT(assert_equal(landmarkL, *actual1, 1e-7));
#endif
}
{
// 2. test with optimization on, same answer
bool optimize = true;
#ifdef GTSAM_THROW_CHEIRALITY_EXCEPTION
CHECK_EXCEPTION(triangulatePoint3<SphericalCamera>(cameras, measurements,
rank_tol, optimize),
TriangulationCheiralityException);
#else // otherwise project should not throw the exception
boost::optional<Point3> actual1 = //
triangulatePoint3<SphericalCamera>(cameras, measurements, rank_tol,
optimize);
EXPECT(assert_equal(landmarkL, *actual1, 1e-7));
#endif
}
}
//******************************************************************************
TEST(triangulation, reprojectionError_cameraComparison) {
Pose3 poseA = Pose3(
Rot3::Ypr(-M_PI / 2, 0., -M_PI / 2),
Point3(0.0, 0.0, 0.0)); // with z pointing along x axis of global frame
Point3 landmarkL(5.0, 0.0, 0.0); // 1m in front of poseA
SphericalCamera sphericalCamera(poseA);
Unit3 u = sphericalCamera.project(landmarkL);
static Cal3_S2::shared_ptr sharedK(new Cal3_S2(60, 640, 480));
PinholePose<Cal3_S2> pinholeCamera(poseA, sharedK);
Vector2 px = pinholeCamera.project(landmarkL);
// add perturbation and compare error in both cameras
Vector2 px_noise(1.0, 2.0); // px perturbation vertically and horizontally
Vector2 measured_px = px + px_noise;
Vector2 measured_px_calibrated = sharedK->calibrate(measured_px);
Unit3 measured_u =
Unit3(measured_px_calibrated[0], measured_px_calibrated[1], 1.0);
Unit3 expected_measured_u =
Unit3(px_noise[0] / sharedK->fx(), px_noise[1] / sharedK->fy(), 1.0);
EXPECT(assert_equal(expected_measured_u, measured_u, 1e-7));
Vector2 actualErrorPinhole =
pinholeCamera.reprojectionError(landmarkL, measured_px);
Vector2 expectedErrorPinhole = Vector2(-px_noise[0], -px_noise[1]);
EXPECT(assert_equal(expectedErrorPinhole, actualErrorPinhole,
1e-7)); //- sign due to definition of error
Vector2 actualErrorSpherical =
sphericalCamera.reprojectionError(landmarkL, measured_u);
// expectedError: not easy to calculate, since it involves the unit3 basis
Vector2 expectedErrorSpherical(-0.00360842, 0.00180419);
EXPECT(assert_equal(expectedErrorSpherical, actualErrorSpherical, 1e-7));
}
//****************************************************************************** //******************************************************************************
int main() { int main() {
TestResult tr; TestResult tr;

50
gtsam/geometry/triangulation.cpp
View File

@ -53,15 +53,57 @@ Vector4 triangulateHomogeneousDLT(
return v; return v;
} }
Point3 triangulateDLT(const std::vector<Matrix34, Eigen::aligned_allocator<Matrix34>>& projection_matrices, Vector4 triangulateHomogeneousDLT(
const std::vector<Matrix34, Eigen::aligned_allocator<Matrix34>>& projection_matrices,
const std::vector<Unit3>& measurements, double rank_tol) {
// number of cameras
size_t m = projection_matrices.size();
// Allocate DLT matrix
Matrix A = Matrix::Zero(m * 2, 4);
for (size_t i = 0; i < m; i++) {
size_t row = i * 2;
const Matrix34& projection = projection_matrices.at(i);
const Point3& p = measurements.at(i).point3(); // to get access to x,y,z of the bearing vector
// build system of equations
A.row(row) = p.x() * projection.row(2) - p.z() * projection.row(0);
A.row(row + 1) = p.y() * projection.row(2) - p.z() * projection.row(1);
}
int rank;
double error;
Vector v;
boost::tie(rank, error, v) = DLT(A, rank_tol);
if (rank < 3)
throw(TriangulationUnderconstrainedException());
return v;
}
Point3 triangulateDLT(
const std::vector<Matrix34, Eigen::aligned_allocator<Matrix34>>& projection_matrices,
const Point2Vector& measurements, double rank_tol) { const Point2Vector& measurements, double rank_tol) {
Vector4 v = triangulateHomogeneousDLT(projection_matrices, measurements, rank_tol); Vector4 v = triangulateHomogeneousDLT(projection_matrices, measurements,
rank_tol);
// Create 3D point from homogeneous coordinates // Create 3D point from homogeneous coordinates
return Point3(v.head<3>() / v[3]); return Point3(v.head<3>() / v[3]);
} }
Point3 triangulateDLT(
const std::vector<Matrix34, Eigen::aligned_allocator<Matrix34>>& projection_matrices,
const std::vector<Unit3>& measurements, double rank_tol) {
// contrary to previous triangulateDLT, this is now taking Unit3 inputs
Vector4 v = triangulateHomogeneousDLT(projection_matrices, measurements,
rank_tol);
// Create 3D point from homogeneous coordinates
return Point3(v.head<3>() / v[3]);
}
/// ///
/** /**
* Optimize for triangulation * Optimize for triangulation
@ -71,7 +113,7 @@ Point3 triangulateDLT(const std::vector<Matrix34, Eigen::aligned_allocator<Matri
* @return refined Point3 * @return refined Point3
*/ */
Point3 optimize(const NonlinearFactorGraph& graph, const Values& values, Point3 optimize(const NonlinearFactorGraph& graph, const Values& values,
Key landmarkKey) { Key landmarkKey) {
// Maybe we should consider Gauss-Newton? // Maybe we should consider Gauss-Newton?
LevenbergMarquardtParams params; LevenbergMarquardtParams params;
params.verbosityLM = LevenbergMarquardtParams::TRYLAMBDA; params.verbosityLM = LevenbergMarquardtParams::TRYLAMBDA;

75
gtsam/geometry/triangulation.h
View File

@ -24,6 +24,7 @@
#include <gtsam/geometry/Cal3_S2.h> #include <gtsam/geometry/Cal3_S2.h>
#include <gtsam/geometry/CameraSet.h> #include <gtsam/geometry/CameraSet.h>
#include <gtsam/geometry/PinholeCamera.h> #include <gtsam/geometry/PinholeCamera.h>
#include <gtsam/geometry/SphericalCamera.h>
#include <gtsam/geometry/Pose2.h> #include <gtsam/geometry/Pose2.h>
#include <gtsam/inference/Symbol.h> #include <gtsam/inference/Symbol.h>
#include <gtsam/nonlinear/NonlinearFactorGraph.h> #include <gtsam/nonlinear/NonlinearFactorGraph.h>
@ -59,6 +60,18 @@ GTSAM_EXPORT Vector4 triangulateHomogeneousDLT(
const std::vector<Matrix34, Eigen::aligned_allocator<Matrix34>>& projection_matrices, const std::vector<Matrix34, Eigen::aligned_allocator<Matrix34>>& projection_matrices,
const Point2Vector& measurements, double rank_tol = 1e-9); const Point2Vector& measurements, double rank_tol = 1e-9);
/**
* Same math as Hartley and Zisserman, 2nd Ed., page 312, but with unit-norm bearing vectors
* (contrarily to pinhole projection, the z entry is not assumed to be 1 as in Hartley and Zisserman)
* @param projection_matrices Projection matrices (K*P^-1)
* @param measurements Unit3 bearing measurements
* @param rank_tol SVD rank tolerance
* @return Triangulated point, in homogeneous coordinates
*/
GTSAM_EXPORT Vector4 triangulateHomogeneousDLT(
const std::vector<Matrix34, Eigen::aligned_allocator<Matrix34>>& projection_matrices,
const std::vector<Unit3>& measurements, double rank_tol = 1e-9);
/** /**
* DLT triangulation: See Hartley and Zisserman, 2nd Ed., page 312 * DLT triangulation: See Hartley and Zisserman, 2nd Ed., page 312
* @param projection_matrices Projection matrices (K*P^-1) * @param projection_matrices Projection matrices (K*P^-1)
@ -71,6 +84,14 @@ GTSAM_EXPORT Point3 triangulateDLT(
const Point2Vector& measurements, const Point2Vector& measurements,
double rank_tol = 1e-9); double rank_tol = 1e-9);
/**
* overload of previous function to work with Unit3 (projected to canonical camera)
*/
GTSAM_EXPORT Point3 triangulateDLT(
const std::vector<Matrix34, Eigen::aligned_allocator<Matrix34>>& projection_matrices,
const std::vector<Unit3>& measurements,
double rank_tol = 1e-9);
/** /**
* Create a factor graph with projection factors from poses and one calibration * Create a factor graph with projection factors from poses and one calibration
* @param poses Camera poses * @param poses Camera poses
@ -180,26 +201,27 @@ Point3 triangulateNonlinear(
return optimize(graph, values, Symbol('p', 0)); return optimize(graph, values, Symbol('p', 0));
} }
/** template<class CAMERA>
* Create a 3*4 camera projection matrix from calibration and pose. std::vector<Matrix34, Eigen::aligned_allocator<Matrix34>>
* Functor for partial application on calibration projectionMatricesFromCameras(const CameraSet<CAMERA> &cameras) {
* @param pose The camera pose std::vector<Matrix34, Eigen::aligned_allocator<Matrix34>> projection_matrices;
* @param cal The calibration for (const CAMERA &camera: cameras) {
* @return Returns a Matrix34 projection_matrices.push_back(camera.cameraProjectionMatrix());
*/ }
return projection_matrices;
}
// overload, assuming pinholePose
template<class CALIBRATION> template<class CALIBRATION>
struct CameraProjectionMatrix { std::vector<Matrix34, Eigen::aligned_allocator<Matrix34>> projectionMatricesFromPoses(
CameraProjectionMatrix(const CALIBRATION& calibration) : const std::vector<Pose3> &poses, boost::shared_ptr<CALIBRATION> sharedCal) {
K_(calibration.K()) { std::vector<Matrix34, Eigen::aligned_allocator<Matrix34>> projection_matrices;
for (size_t i = 0; i < poses.size(); i++) {
PinholePose<CALIBRATION> camera(poses.at(i), sharedCal);
projection_matrices.push_back(camera.cameraProjectionMatrix());
} }
Matrix34 operator()(const Pose3& pose) const { return projection_matrices;
return K_ * (pose.inverse().matrix()).block<3, 4>(0, 0); }
}
private:
const Matrix3 K_;
public:
GTSAM_MAKE_ALIGNED_OPERATOR_NEW
};
/** /**
* Function to triangulate 3D landmark point from an arbitrary number * Function to triangulate 3D landmark point from an arbitrary number
@ -224,10 +246,7 @@ Point3 triangulatePoint3(const std::vector<Pose3>& poses,
throw(TriangulationUnderconstrainedException()); throw(TriangulationUnderconstrainedException());
// construct projection matrices from poses & calibration // construct projection matrices from poses & calibration
std::vector<Matrix34, Eigen::aligned_allocator<Matrix34>> projection_matrices; auto projection_matrices = projectionMatricesFromPoses(poses, sharedCal);
CameraProjectionMatrix<CALIBRATION> createP(*sharedCal); // partially apply
for(const Pose3& pose: poses)
projection_matrices.push_back(createP(pose));
// Triangulate linearly // Triangulate linearly
Point3 point = triangulateDLT(projection_matrices, measurements, rank_tol); Point3 point = triangulateDLT(projection_matrices, measurements, rank_tol);
@ -274,11 +293,7 @@ Point3 triangulatePoint3(
throw(TriangulationUnderconstrainedException()); throw(TriangulationUnderconstrainedException());
// construct projection matrices from poses & calibration // construct projection matrices from poses & calibration
std::vector<Matrix34, Eigen::aligned_allocator<Matrix34>> projection_matrices; auto projection_matrices = projectionMatricesFromCameras(cameras);
for(const CAMERA& camera: cameras)
projection_matrices.push_back(
CameraProjectionMatrix<typename CAMERA::CalibrationType>(camera.calibration())(
camera.pose()));
Point3 point = triangulateDLT(projection_matrices, measurements, rank_tol); Point3 point = triangulateDLT(projection_matrices, measurements, rank_tol);
// The n refine using non-linear optimization // The n refine using non-linear optimization
@ -474,8 +489,8 @@ TriangulationResult triangulateSafe(const CameraSet<CAMERA>& cameras,
#endif #endif
// Check reprojection error // Check reprojection error
if (params.dynamicOutlierRejectionThreshold > 0) { if (params.dynamicOutlierRejectionThreshold > 0) {
const Point2& zi = measured.at(i); const typename CAMERA::Measurement& zi = measured.at(i);
Point2 reprojectionError(camera.project(point) - zi); Point2 reprojectionError = camera.reprojectionError(point, zi);
maxReprojError = std::max(maxReprojError, reprojectionError.norm()); maxReprojError = std::max(maxReprojError, reprojectionError.norm());
} }
i += 1; i += 1;
@ -503,6 +518,6 @@ using CameraSetCal3Bundler = CameraSet<PinholeCamera<Cal3Bundler>>;
using CameraSetCal3_S2 = CameraSet<PinholeCamera<Cal3_S2>>; using CameraSetCal3_S2 = CameraSet<PinholeCamera<Cal3_S2>>;
using CameraSetCal3Fisheye = CameraSet<PinholeCamera<Cal3Fisheye>>; using CameraSetCal3Fisheye = CameraSet<PinholeCamera<Cal3Fisheye>>;
using CameraSetCal3Unified = CameraSet<PinholeCamera<Cal3Unified>>; using CameraSetCal3Unified = CameraSet<PinholeCamera<Cal3Unified>>;
using CameraSetSpherical = CameraSet<SphericalCamera>;
} // \namespace gtsam } // \namespace gtsam

2
gtsam/inference/BayesTree.h
View File

@ -19,6 +19,8 @@
#pragma once #pragma once
#include <boost/shared_ptr.hpp>
#include <gtsam/inference/Key.h> #include <gtsam/inference/Key.h>
#include <gtsam/base/FastList.h> #include <gtsam/base/FastList.h>
#include <gtsam/base/ConcurrentMap.h> #include <gtsam/base/ConcurrentMap.h>

2
gtsam/inference/ClusterTree.h
View File

@ -110,7 +110,7 @@ class ClusterTree {
typedef sharedCluster sharedNode; typedef sharedCluster sharedNode;
/** concept check */ /** concept check */
GTSAM_CONCEPT_TESTABLE_TYPE(FactorType); GTSAM_CONCEPT_TESTABLE_TYPE(FactorType)
protected: protected:
FastVector<sharedNode> roots_; FastVector<sharedNode> roots_;

48
gtsam/inference/EliminateableFactorGraph-inst.h
View File

@ -36,17 +36,17 @@ namespace gtsam {
// no Ordering is provided. When removing optional from VariableIndex, create VariableIndex // no Ordering is provided. When removing optional from VariableIndex, create VariableIndex
// before creating ordering. // before creating ordering.
VariableIndex computedVariableIndex(asDerived()); VariableIndex computedVariableIndex(asDerived());
return eliminateSequential(function, computedVariableIndex, orderingType); return eliminateSequential(orderingType, function, computedVariableIndex);
} }
else { else {
// Compute an ordering and call this function again. We are guaranteed to have a // Compute an ordering and call this function again. We are guaranteed to have a
// VariableIndex already here because we computed one if needed in the previous 'if' block. // VariableIndex already here because we computed one if needed in the previous 'if' block.
if (orderingType == Ordering::METIS) { if (orderingType == Ordering::METIS) {
Ordering computedOrdering = Ordering::Metis(asDerived()); Ordering computedOrdering = Ordering::Metis(asDerived());
return eliminateSequential(computedOrdering, function, variableIndex, orderingType); return eliminateSequential(computedOrdering, function, variableIndex);
} else { } else {
Ordering computedOrdering = Ordering::Colamd(*variableIndex); Ordering computedOrdering = Ordering::Colamd(*variableIndex);
return eliminateSequential(computedOrdering, function, variableIndex, orderingType); return eliminateSequential(computedOrdering, function, variableIndex);
} }
} }
} }
@ -78,29 +78,31 @@ namespace gtsam {
} }
/* ************************************************************************* */ /* ************************************************************************* */
template<class FACTORGRAPH> template <class FACTORGRAPH>
boost::shared_ptr<typename EliminateableFactorGraph<FACTORGRAPH>::BayesTreeType> boost::shared_ptr<
EliminateableFactorGraph<FACTORGRAPH>::eliminateMultifrontal( typename EliminateableFactorGraph<FACTORGRAPH>::BayesTreeType>
OptionalOrderingType orderingType, const Eliminate& function, EliminateableFactorGraph<FACTORGRAPH>::eliminateMultifrontal(
OptionalVariableIndex variableIndex) const OptionalOrderingType orderingType, const Eliminate& function,
{ OptionalVariableIndex variableIndex) const {
if(!variableIndex) { if (!variableIndex) {
// If no VariableIndex provided, compute one and call this function again IMPORTANT: we check // If no VariableIndex provided, compute one and call this function again
// for no variable index first so that it's always computed if we need to call COLAMD because // IMPORTANT: we check for no variable index first so that it's always
// no Ordering is provided. When removing optional from VariableIndex, create VariableIndex // computed if we need to call COLAMD because no Ordering is provided.
// before creating ordering. // When removing optional from VariableIndex, create VariableIndex before
// creating ordering.
VariableIndex computedVariableIndex(asDerived()); VariableIndex computedVariableIndex(asDerived());
return eliminateMultifrontal(function, computedVariableIndex, orderingType); return eliminateMultifrontal(orderingType, function,
} computedVariableIndex);
else { } else {
// Compute an ordering and call this function again. We are guaranteed to have a // Compute an ordering and call this function again. We are guaranteed to
// VariableIndex already here because we computed one if needed in the previous 'if' block. // have a VariableIndex already here because we computed one if needed in
// the previous 'if' block.
if (orderingType == Ordering::METIS) { if (orderingType == Ordering::METIS) {
Ordering computedOrdering = Ordering::Metis(asDerived()); Ordering computedOrdering = Ordering::Metis(asDerived());
return eliminateMultifrontal(computedOrdering, function, variableIndex, orderingType); return eliminateMultifrontal(computedOrdering, function, variableIndex);
} else { } else {
Ordering computedOrdering = Ordering::Colamd(*variableIndex); Ordering computedOrdering = Ordering::Colamd(*variableIndex);
return eliminateMultifrontal(computedOrdering, function, variableIndex, orderingType); return eliminateMultifrontal(computedOrdering, function, variableIndex);
} }
} }
} }
@ -273,7 +275,7 @@ namespace gtsam {
else else
{ {
// No ordering was provided for the unmarginalized variables, so order them with COLAMD. // No ordering was provided for the unmarginalized variables, so order them with COLAMD.
return factorGraph->eliminateSequential(function); return factorGraph->eliminateSequential(Ordering::COLAMD, function);
} }
} }
} }
@ -340,7 +342,7 @@ namespace gtsam {
else else
{ {
// No ordering was provided for the unmarginalized variables, so order them with COLAMD. // No ordering was provided for the unmarginalized variables, so order them with COLAMD.
return factorGraph->eliminateMultifrontal(function); return factorGraph->eliminateMultifrontal(Ordering::COLAMD, function);
} }
} }
} }

14
gtsam/inference/EliminateableFactorGraph.h
View File

@ -288,8 +288,9 @@ namespace gtsam {
FactorGraphType& asDerived() { return static_cast<FactorGraphType&>(*this); } FactorGraphType& asDerived() { return static_cast<FactorGraphType&>(*this); }
public: public:
#ifdef GTSAM_ALLOW_DEPRECATED_SINCE_V41
/** \deprecated ordering and orderingType shouldn't both be specified */ /** \deprecated ordering and orderingType shouldn't both be specified */
boost::shared_ptr<BayesNetType> eliminateSequential( boost::shared_ptr<BayesNetType> GTSAM_DEPRECATED eliminateSequential(
const Ordering& ordering, const Ordering& ordering,
const Eliminate& function, const Eliminate& function,
OptionalVariableIndex variableIndex, OptionalVariableIndex variableIndex,
@ -298,7 +299,7 @@ namespace gtsam {
} }
/** \deprecated orderingType specified first for consistency */ /** \deprecated orderingType specified first for consistency */
boost::shared_ptr<BayesNetType> eliminateSequential( boost::shared_ptr<BayesNetType> GTSAM_DEPRECATED eliminateSequential(
const Eliminate& function, const Eliminate& function,
OptionalVariableIndex variableIndex = boost::none, OptionalVariableIndex variableIndex = boost::none,
OptionalOrderingType orderingType = boost::none) const { OptionalOrderingType orderingType = boost::none) const {
@ -306,7 +307,7 @@ namespace gtsam {
} }
/** \deprecated ordering and orderingType shouldn't both be specified */ /** \deprecated ordering and orderingType shouldn't both be specified */
boost::shared_ptr<BayesTreeType> eliminateMultifrontal( boost::shared_ptr<BayesTreeType> GTSAM_DEPRECATED eliminateMultifrontal(
const Ordering& ordering, const Ordering& ordering,
const Eliminate& function, const Eliminate& function,
OptionalVariableIndex variableIndex, OptionalVariableIndex variableIndex,
@ -315,7 +316,7 @@ namespace gtsam {
} }
/** \deprecated orderingType specified first for consistency */ /** \deprecated orderingType specified first for consistency */
boost::shared_ptr<BayesTreeType> eliminateMultifrontal( boost::shared_ptr<BayesTreeType> GTSAM_DEPRECATED eliminateMultifrontal(
const Eliminate& function, const Eliminate& function,
OptionalVariableIndex variableIndex = boost::none, OptionalVariableIndex variableIndex = boost::none,
OptionalOrderingType orderingType = boost::none) const { OptionalOrderingType orderingType = boost::none) const {
@ -323,7 +324,7 @@ namespace gtsam {
} }
/** \deprecated */ /** \deprecated */
boost::shared_ptr<BayesNetType> marginalMultifrontalBayesNet( boost::shared_ptr<BayesNetType> GTSAM_DEPRECATED marginalMultifrontalBayesNet(
boost::variant<const Ordering&, const KeyVector&> variables, boost::variant<const Ordering&, const KeyVector&> variables,
boost::none_t, boost::none_t,
const Eliminate& function = EliminationTraitsType::DefaultEliminate, const Eliminate& function = EliminationTraitsType::DefaultEliminate,
@ -332,13 +333,14 @@ namespace gtsam {
} }
/** \deprecated */ /** \deprecated */
boost::shared_ptr<BayesTreeType> marginalMultifrontalBayesTree( boost::shared_ptr<BayesTreeType> GTSAM_DEPRECATED marginalMultifrontalBayesTree(
boost::variant<const Ordering&, const KeyVector&> variables, boost::variant<const Ordering&, const KeyVector&> variables,
boost::none_t, boost::none_t,
const Eliminate& function = EliminationTraitsType::DefaultEliminate, const Eliminate& function = EliminationTraitsType::DefaultEliminate,
OptionalVariableIndex variableIndex = boost::none) const { OptionalVariableIndex variableIndex = boost::none) const {
return marginalMultifrontalBayesTree(variables, function, variableIndex); return marginalMultifrontalBayesTree(variables, function, variableIndex);
} }
#endif
}; };
} }

2
gtsam/inference/EliminationTree.h
View File

@ -81,7 +81,7 @@ namespace gtsam {
protected: protected:
/** concept check */ /** concept check */
GTSAM_CONCEPT_TESTABLE_TYPE(FactorType); GTSAM_CONCEPT_TESTABLE_TYPE(FactorType)
FastVector<sharedNode> roots_; FastVector<sharedNode> roots_;
FastVector<sharedFactor> remainingFactors_; FastVector<sharedFactor> remainingFactors_;

1
gtsam/inference/Factor.h
View File

@ -22,6 +22,7 @@
#pragma once #pragma once
#include <boost/serialization/nvp.hpp> #include <boost/serialization/nvp.hpp>
#include <boost/shared_ptr.hpp>
#include <gtsam/base/types.h> #include <gtsam/base/types.h>
#include <gtsam/base/FastVector.h> #include <gtsam/base/FastVector.h>

4
gtsam/linear/Errors.cpp
View File

@ -111,10 +111,10 @@ double dot(const Errors& a, const Errors& b) {
/* ************************************************************************* */ /* ************************************************************************* */
template<> template<>
void axpy<Errors,Errors>(double alpha, const Errors& x, Errors& y) { void axpy(double alpha, const Errors& x, Errors& y) {
Errors::const_iterator it = x.begin(); Errors::const_iterator it = x.begin();
for(Vector& yi: y) for(Vector& yi: y)
axpy(alpha,*(it++),yi); yi += alpha * (*(it++));
} }
/* ************************************************************************* */ /* ************************************************************************* */

2
gtsam/linear/Errors.h
View File

@ -66,7 +66,7 @@ namespace gtsam {
* BLAS level 2 style * BLAS level 2 style
*/ */
template <> template <>
GTSAM_EXPORT void axpy<Errors,Errors>(double alpha, const Errors& x, Errors& y); GTSAM_EXPORT void axpy(double alpha, const Errors& x, Errors& y);
/** print with optional string */ /** print with optional string */
GTSAM_EXPORT void print(const Errors& a, const std::string& s = "Error"); GTSAM_EXPORT void print(const Errors& a, const std::string& s = "Error");

36
gtsam/linear/GaussianFactorGraph.cpp
View File

@ -19,7 +19,6 @@
*/ */
#include <gtsam/linear/GaussianFactorGraph.h> #include <gtsam/linear/GaussianFactorGraph.h>
#include <gtsam/linear/VectorValues.h>
#include <gtsam/linear/GaussianBayesTree.h> #include <gtsam/linear/GaussianBayesTree.h>
#include <gtsam/linear/GaussianEliminationTree.h> #include <gtsam/linear/GaussianEliminationTree.h>
#include <gtsam/linear/GaussianJunctionTree.h> #include <gtsam/linear/GaussianJunctionTree.h>
@ -290,10 +289,11 @@ namespace gtsam {
return blocks; return blocks;
} }
/* ************************************************************************* */ /* ************************************************************************ */
VectorValues GaussianFactorGraph::optimize(const Eliminate& function) const { VectorValues GaussianFactorGraph::optimize(const Eliminate& function) const {
gttic(GaussianFactorGraph_optimize); gttic(GaussianFactorGraph_optimize);
return BaseEliminateable::eliminateMultifrontal(function)->optimize(); return BaseEliminateable::eliminateMultifrontal(Ordering::COLAMD, function)
->optimize();
} }
/* ************************************************************************* */ /* ************************************************************************* */
@ -504,10 +504,32 @@ namespace gtsam {
} }
/* ************************************************************************* */ /* ************************************************************************* */
/** \deprecated */ void GaussianFactorGraph::printErrors(
VectorValues GaussianFactorGraph::optimize(boost::none_t, const VectorValues& values, const std::string& str,
const Eliminate& function) const { const KeyFormatter& keyFormatter,
return optimize(function); const std::function<bool(const Factor* /*factor*/,
double /*whitenedError*/, size_t /*index*/)>&
printCondition) const {
cout << str << "size: " << size() << endl << endl;
for (size_t i = 0; i < (*this).size(); i++) {
const sharedFactor& factor = (*this)[i];
const double errorValue =
(factor != nullptr ? (*this)[i]->error(values) : .0);
if (!printCondition(factor.get(), errorValue, i))
continue; // User-provided filter did not pass
stringstream ss;
ss << "Factor " << i << ": ";
if (factor == nullptr) {
cout << "nullptr"
<< "\n";
} else {
factor->print(ss.str(), keyFormatter);
cout << "error = " << errorValue << "\n";
}
cout << endl; // only one "endl" at end might be faster, \n for each
// factor
}
} }
} // namespace gtsam } // namespace gtsam

26
gtsam/linear/GaussianFactorGraph.h
View File

@ -21,12 +21,13 @@
#pragma once #pragma once
#include <gtsam/inference/FactorGraph.h>
#include <gtsam/inference/EliminateableFactorGraph.h> #include <gtsam/inference/EliminateableFactorGraph.h>
#include <gtsam/inference/FactorGraph.h>
#include <gtsam/linear/Errors.h> // Included here instead of fw-declared so we can use Errors::iterator
#include <gtsam/linear/GaussianFactor.h> #include <gtsam/linear/GaussianFactor.h>
#include <gtsam/linear/JacobianFactor.h>
#include <gtsam/linear/HessianFactor.h> #include <gtsam/linear/HessianFactor.h>
#include <gtsam/linear/Errors.h> // Included here instead of fw-declared so we can use Errors::iterator #include <gtsam/linear/JacobianFactor.h>
#include <gtsam/linear/VectorValues.h>
namespace gtsam { namespace gtsam {
@ -375,6 +376,14 @@ namespace gtsam {
/** In-place version e <- A*x that takes an iterator. */ /** In-place version e <- A*x that takes an iterator. */
void multiplyInPlace(const VectorValues& x, const Errors::iterator& e) const; void multiplyInPlace(const VectorValues& x, const Errors::iterator& e) const;
void printErrors(
const VectorValues& x,
const std::string& str = "GaussianFactorGraph: ",
const KeyFormatter& keyFormatter = DefaultKeyFormatter,
const std::function<bool(const Factor* /*factor*/,
double /*whitenedError*/, size_t /*index*/)>&
printCondition =
[](const Factor*, double, size_t) { return true; }) const;
/// @} /// @}
private: private:
@ -387,9 +396,14 @@ namespace gtsam {
public: public:
/** \deprecated */ #ifdef GTSAM_ALLOW_DEPRECATED_SINCE_V41
VectorValues optimize(boost::none_t, /** \deprecated */
const Eliminate& function = EliminationTraitsType::DefaultEliminate) const; VectorValues optimize(boost::none_t,
const Eliminate& function =
EliminationTraitsType::DefaultEliminate) const {
return optimize(function);
}
#endif
}; };

54
gtsam/linear/NoiseModel.h
View File

@ -177,17 +177,16 @@ namespace gtsam {
return *sqrt_information_; return *sqrt_information_;
} }
protected:
/** protected constructor takes square root information matrix */
Gaussian(size_t dim = 1, const boost::optional<Matrix>& sqrt_information = boost::none) :
Base(dim), sqrt_information_(sqrt_information) {
}
public: public:
typedef boost::shared_ptr<Gaussian> shared_ptr; typedef boost::shared_ptr<Gaussian> shared_ptr;
/** constructor takes square root information matrix */
Gaussian(size_t dim = 1,
const boost::optional<Matrix>& sqrt_information = boost::none)
: Base(dim), sqrt_information_(sqrt_information) {}
~Gaussian() override {} ~Gaussian() override {}
/** /**
@ -290,13 +289,13 @@ namespace gtsam {
Vector sigmas_, invsigmas_, precisions_; Vector sigmas_, invsigmas_, precisions_;
protected: protected:
/** protected constructor - no initializations */
Diagonal();
/** constructor to allow for disabling initialization of invsigmas */ /** constructor to allow for disabling initialization of invsigmas */
Diagonal(const Vector& sigmas); Diagonal(const Vector& sigmas);
public: public:
/** constructor - no initializations, for serialization */
Diagonal();
typedef boost::shared_ptr<Diagonal> shared_ptr; typedef boost::shared_ptr<Diagonal> shared_ptr;
@ -387,14 +386,6 @@ namespace gtsam {
// Sigmas are contained in the base class // Sigmas are contained in the base class
Vector mu_; ///< Penalty function weight - needs to be large enough to dominate soft constraints Vector mu_; ///< Penalty function weight - needs to be large enough to dominate soft constraints
/**
* protected constructor takes sigmas.
* prevents any inf values
* from appearing in invsigmas or precisions.
* mu set to large default value (1000.0)
*/
Constrained(const Vector& sigmas = Z_1x1);
/** /**
* Constructor that prevents any inf values * Constructor that prevents any inf values
* from appearing in invsigmas or precisions. * from appearing in invsigmas or precisions.
@ -406,6 +397,14 @@ namespace gtsam {
typedef boost::shared_ptr<Constrained> shared_ptr; typedef boost::shared_ptr<Constrained> shared_ptr;
/**
* protected constructor takes sigmas.
* prevents any inf values
* from appearing in invsigmas or precisions.
* mu set to large default value (1000.0)
*/
Constrained(const Vector& sigmas = Z_1x1);
~Constrained() override {} ~Constrained() override {}
/// true if a constrained noise mode, saves slow/clumsy dynamic casting /// true if a constrained noise mode, saves slow/clumsy dynamic casting
@ -461,6 +460,11 @@ namespace gtsam {
return MixedVariances(precisions.array().inverse()); return MixedVariances(precisions.array().inverse());
} }
/**
* The squaredMahalanobisDistance function for a constrained noisemodel,
* for non-constrained versions, uses sigmas, otherwise
* uses the penalty function with mu
*/
double squaredMahalanobisDistance(const Vector& v) const override; double squaredMahalanobisDistance(const Vector& v) const override;
/** Fully constrained variations */ /** Fully constrained variations */
@ -531,11 +535,11 @@ namespace gtsam {
Isotropic(size_t dim, double sigma) : Isotropic(size_t dim, double sigma) :
Diagonal(Vector::Constant(dim, sigma)),sigma_(sigma),invsigma_(1.0/sigma) {} Diagonal(Vector::Constant(dim, sigma)),sigma_(sigma),invsigma_(1.0/sigma) {}
public:
/* dummy constructor to allow for serialization */ /* dummy constructor to allow for serialization */
Isotropic() : Diagonal(Vector1::Constant(1.0)),sigma_(1.0),invsigma_(1.0) {} Isotropic() : Diagonal(Vector1::Constant(1.0)),sigma_(1.0),invsigma_(1.0) {}
public:
~Isotropic() override {} ~Isotropic() override {}
typedef boost::shared_ptr<Isotropic> shared_ptr; typedef boost::shared_ptr<Isotropic> shared_ptr;
@ -592,14 +596,13 @@ namespace gtsam {
* Unit: i.i.d. unit-variance noise on all m dimensions. * Unit: i.i.d. unit-variance noise on all m dimensions.
*/ */
class GTSAM_EXPORT Unit : public Isotropic { class GTSAM_EXPORT Unit : public Isotropic {
protected:
Unit(size_t dim=1): Isotropic(dim,1.0) {}
public: public:
typedef boost::shared_ptr<Unit> shared_ptr; typedef boost::shared_ptr<Unit> shared_ptr;
/** constructor for serialization */
Unit(size_t dim=1): Isotropic(dim,1.0) {}
~Unit() override {} ~Unit() override {}
/** /**
@ -682,19 +685,19 @@ namespace gtsam {
/// Return the contained noise model /// Return the contained noise model
const NoiseModel::shared_ptr& noise() const { return noise_; } const NoiseModel::shared_ptr& noise() const { return noise_; }
// TODO: functions below are dummy but necessary for the noiseModel::Base // Functions below are dummy but necessary for the noiseModel::Base
inline Vector whiten(const Vector& v) const override inline Vector whiten(const Vector& v) const override
{ Vector r = v; this->WhitenSystem(r); return r; } { Vector r = v; this->WhitenSystem(r); return r; }
inline Matrix Whiten(const Matrix& A) const override inline Matrix Whiten(const Matrix& A) const override
{ Vector b; Matrix B=A; this->WhitenSystem(B,b); return B; } { Vector b; Matrix B=A; this->WhitenSystem(B,b); return B; }
inline Vector unwhiten(const Vector& /*v*/) const override inline Vector unwhiten(const Vector& /*v*/) const override
{ throw std::invalid_argument("unwhiten is not currently supported for robust noise models."); } { throw std::invalid_argument("unwhiten is not currently supported for robust noise models."); }
/// Compute loss from the m-estimator using the Mahalanobis distance.
double loss(const double squared_distance) const override { double loss(const double squared_distance) const override {
return robust_->loss(std::sqrt(squared_distance)); return robust_->loss(std::sqrt(squared_distance));
} }
// TODO: these are really robust iterated re-weighting support functions // These are really robust iterated re-weighting support functions
virtual void WhitenSystem(Vector& b) const; virtual void WhitenSystem(Vector& b) const;
void WhitenSystem(std::vector<Matrix>& A, Vector& b) const override; void WhitenSystem(std::vector<Matrix>& A, Vector& b) const override;
void WhitenSystem(Matrix& A, Vector& b) const override; void WhitenSystem(Matrix& A, Vector& b) const override;
@ -705,7 +708,6 @@ namespace gtsam {
return noise_->unweightedWhiten(v); return noise_->unweightedWhiten(v);
} }
double weight(const Vector& v) const override { double weight(const Vector& v) const override {
// Todo(mikebosse): make the robust weight function input a vector.
return robust_->weight(v.norm()); return robust_->weight(v.norm());
} }

4
gtsam/linear/SubgraphPreconditioner.cpp
View File

@ -173,7 +173,7 @@ void SubgraphPreconditioner::transposeMultiplyAdd
Errors::const_iterator it = e.begin(); Errors::const_iterator it = e.begin();
for(auto& key_value: y) { for(auto& key_value: y) {
const Vector& ei = *it; const Vector& ei = *it;
axpy(alpha, ei, key_value.second); key_value.second += alpha * ei;
++it; ++it;
} }
transposeMultiplyAdd2(alpha, it, e.end(), y); transposeMultiplyAdd2(alpha, it, e.end(), y);
@ -191,7 +191,7 @@ void SubgraphPreconditioner::transposeMultiplyAdd2 (double alpha,
VectorValues x = VectorValues::Zero(y); // x = 0 VectorValues x = VectorValues::Zero(y); // x = 0
Ab2_->transposeMultiplyAdd(1.0,e2,x); // x += A2'*e2 Ab2_->transposeMultiplyAdd(1.0,e2,x); // x += A2'*e2
axpy(alpha, Rc1_->backSubstituteTranspose(x), y); // y += alpha*inv(R1')*x y += alpha * Rc1_->backSubstituteTranspose(x); // y += alpha*inv(R1')*x
} }
/* ************************************************************************* */ /* ************************************************************************* */

4
gtsam/linear/iterative-inl.h
View File

@ -72,7 +72,7 @@ namespace gtsam {
double takeOptimalStep(V& x) { double takeOptimalStep(V& x) {
// TODO: can we use gamma instead of dot(d,g) ????? Answer not trivial // TODO: can we use gamma instead of dot(d,g) ????? Answer not trivial
double alpha = -dot(d, g) / dot(Ad, Ad); // calculate optimal step-size double alpha = -dot(d, g) / dot(Ad, Ad); // calculate optimal step-size
axpy(alpha, d, x); // // do step in new search direction, x += alpha*d x += alpha * d; // do step in new search direction, x += alpha*d
return alpha; return alpha;
} }
@ -106,7 +106,7 @@ namespace gtsam {
double beta = new_gamma / gamma; double beta = new_gamma / gamma;
// d = g + d*beta; // d = g + d*beta;
d *= beta; d *= beta;
axpy(1.0, g, d); d += 1.0 * g;
} }
gamma = new_gamma; gamma = new_gamma;

20
gtsam/linear/linear.i
View File

@ -34,6 +34,9 @@ virtual class Gaussian : gtsam::noiseModel::Base {
// enabling serialization functionality // enabling serialization functionality
void serializable() const; void serializable() const;
// enable pickling in python
void pickle() const;
}; };
virtual class Diagonal : gtsam::noiseModel::Gaussian { virtual class Diagonal : gtsam::noiseModel::Gaussian {
@ -49,6 +52,9 @@ virtual class Diagonal : gtsam::noiseModel::Gaussian {
// enabling serialization functionality // enabling serialization functionality
void serializable() const; void serializable() const;
// enable pickling in python
void pickle() const;
}; };
virtual class Constrained : gtsam::noiseModel::Diagonal { virtual class Constrained : gtsam::noiseModel::Diagonal {
@ -66,6 +72,9 @@ virtual class Constrained : gtsam::noiseModel::Diagonal {
// enabling serialization functionality // enabling serialization functionality
void serializable() const; void serializable() const;
// enable pickling in python
void pickle() const;
}; };
virtual class Isotropic : gtsam::noiseModel::Diagonal { virtual class Isotropic : gtsam::noiseModel::Diagonal {
@ -78,6 +87,9 @@ virtual class Isotropic : gtsam::noiseModel::Diagonal {
// enabling serialization functionality // enabling serialization functionality
void serializable() const; void serializable() const;
// enable pickling in python
void pickle() const;
}; };
virtual class Unit : gtsam::noiseModel::Isotropic { virtual class Unit : gtsam::noiseModel::Isotropic {
@ -85,6 +97,9 @@ virtual class Unit : gtsam::noiseModel::Isotropic {
// enabling serialization functionality // enabling serialization functionality
void serializable() const; void serializable() const;
// enable pickling in python
void pickle() const;
}; };
namespace mEstimator { namespace mEstimator {
@ -221,6 +236,7 @@ class VectorValues {
//Constructors //Constructors
VectorValues(); VectorValues();
VectorValues(const gtsam::VectorValues& other); VectorValues(const gtsam::VectorValues& other);
VectorValues(const gtsam::VectorValues& first, const gtsam::VectorValues& second);
//Named Constructors //Named Constructors
static gtsam::VectorValues Zero(const gtsam::VectorValues& model); static gtsam::VectorValues Zero(const gtsam::VectorValues& model);
@ -301,6 +317,7 @@ virtual class JacobianFactor : gtsam::GaussianFactor {
void print(string s = "", const gtsam::KeyFormatter& keyFormatter = void print(string s = "", const gtsam::KeyFormatter& keyFormatter =
gtsam::DefaultKeyFormatter) const; gtsam::DefaultKeyFormatter) const;
void printKeys(string s) const; void printKeys(string s) const;
gtsam::KeyVector& keys() const;
bool equals(const gtsam::GaussianFactor& lf, double tol) const; bool equals(const gtsam::GaussianFactor& lf, double tol) const;
size_t size() const; size_t size() const;
Vector unweighted_error(const gtsam::VectorValues& c) const; Vector unweighted_error(const gtsam::VectorValues& c) const;
@ -400,6 +417,7 @@ class GaussianFactorGraph {
// error and probability // error and probability
double error(const gtsam::VectorValues& c) const; double error(const gtsam::VectorValues& c) const;
double probPrime(const gtsam::VectorValues& c) const; double probPrime(const gtsam::VectorValues& c) const;
void printErrors(const gtsam::VectorValues& c, string str = "GaussianFactorGraph: ", const gtsam::KeyFormatter& keyFormatter = gtsam::DefaultKeyFormatter) const;
gtsam::GaussianFactorGraph clone() const; gtsam::GaussianFactorGraph clone() const;
gtsam::GaussianFactorGraph negate() const; gtsam::GaussianFactorGraph negate() const;
@ -512,9 +530,9 @@ virtual class GaussianBayesNet {
size_t size() const; size_t size() const;
// FactorGraph derived interface // FactorGraph derived interface
// size_t size() const;
gtsam::GaussianConditional* at(size_t idx) const; gtsam::GaussianConditional* at(size_t idx) const;
gtsam::KeySet keys() const; gtsam::KeySet keys() const;
gtsam::KeyVector keyVector() const;
bool exists(size_t idx) const; bool exists(size_t idx) const;
void saveGraph(const string& s) const; void saveGraph(const string& s) const;

2
gtsam/linear/tests/testErrors.cpp
View File

@ -32,7 +32,7 @@ TEST( Errors, arithmetic )
e += Vector2(1.0,2.0), Vector3(3.0,4.0,5.0); e += Vector2(1.0,2.0), Vector3(3.0,4.0,5.0);
DOUBLES_EQUAL(1+4+9+16+25,dot(e,e),1e-9); DOUBLES_EQUAL(1+4+9+16+25,dot(e,e),1e-9);
axpy(2.0,e,e); axpy(2.0, e, e);
Errors expected; Errors expected;
expected += Vector2(3.0,6.0), Vector3(9.0,12.0,15.0); expected += Vector2(3.0,6.0), Vector3(9.0,12.0,15.0);
CHECK(assert_equal(expected,e)); CHECK(assert_equal(expected,e));

31
gtsam/linear/tests/testNoiseModel.cpp
View File

@ -662,25 +662,14 @@ TEST(NoiseModel, robustNoiseL2WithDeadZone)
{ {
double dead_zone_size = 1.0; double dead_zone_size = 1.0;
SharedNoiseModel robust = noiseModel::Robust::Create( SharedNoiseModel robust = noiseModel::Robust::Create(
noiseModel::mEstimator::L2WithDeadZone::Create(dead_zone_size), noiseModel::mEstimator::L2WithDeadZone::Create(dead_zone_size),
Unit::Create(3)); Unit::Create(3));
/*
* TODO(mike): There is currently a bug in GTSAM, where none of the mEstimator classes
* implement a loss function, and GTSAM calls the weight function to evaluate the
* total penalty, rather than calling the loss function. The weight function should be
* used during iteratively reweighted least squares optimization, but should not be used to
* evaluate the total penalty. The long-term solution is for all mEstimators to implement
* both a weight and a loss function, and for GTSAM to call the loss function when
* evaluating the total penalty. This bug causes the test below to fail, so I'm leaving it
* commented out until the underlying bug in GTSAM is fixed.
*
* for (int i = 0; i < 5; i++) {
* Vector3 error = Vector3(i, 0, 0);
* DOUBLES_EQUAL(0.5*max(0,i-1)*max(0,i-1), robust->distance(error), 1e-8);
* }
*/
for (int i = 0; i < 5; i++) {
Vector3 error = Vector3(i, 0, 0);
DOUBLES_EQUAL(std::fmax(0, i - dead_zone_size) * i,
robust->squaredMahalanobisDistance(error), 1e-8);
}
} }
TEST(NoiseModel, lossFunctionAtZero) TEST(NoiseModel, lossFunctionAtZero)
@ -707,9 +696,9 @@ TEST(NoiseModel, lossFunctionAtZero)
auto dcs = mEstimator::DCS::Create(k); auto dcs = mEstimator::DCS::Create(k);
DOUBLES_EQUAL(dcs->loss(0), 0, 1e-8); DOUBLES_EQUAL(dcs->loss(0), 0, 1e-8);
DOUBLES_EQUAL(dcs->weight(0), 1, 1e-8); DOUBLES_EQUAL(dcs->weight(0), 1, 1e-8);
// auto lsdz = mEstimator::L2WithDeadZone::Create(k); auto lsdz = mEstimator::L2WithDeadZone::Create(k);
// DOUBLES_EQUAL(lsdz->loss(0), 0, 1e-8); DOUBLES_EQUAL(lsdz->loss(0), 0, 1e-8);
// DOUBLES_EQUAL(lsdz->weight(0), 1, 1e-8); DOUBLES_EQUAL(lsdz->weight(0), 0, 1e-8);
} }

20
gtsam/linear/tests/testSerializationLinear.cpp
View File

@ -39,14 +39,14 @@ using namespace gtsam::serializationTestHelpers;
/* ************************************************************************* */ /* ************************************************************************* */
// Export Noisemodels // Export Noisemodels
// See http://www.boost.org/doc/libs/1_32_0/libs/serialization/doc/special.html // See http://www.boost.org/doc/libs/1_32_0/libs/serialization/doc/special.html
BOOST_CLASS_EXPORT_GUID(gtsam::noiseModel::Constrained, "gtsam_noiseModel_Constrained"); BOOST_CLASS_EXPORT_GUID(gtsam::noiseModel::Constrained, "gtsam_noiseModel_Constrained")
BOOST_CLASS_EXPORT_GUID(gtsam::noiseModel::Diagonal, "gtsam_noiseModel_Diagonal"); BOOST_CLASS_EXPORT_GUID(gtsam::noiseModel::Diagonal, "gtsam_noiseModel_Diagonal")
BOOST_CLASS_EXPORT_GUID(gtsam::noiseModel::Gaussian, "gtsam_noiseModel_Gaussian"); BOOST_CLASS_EXPORT_GUID(gtsam::noiseModel::Gaussian, "gtsam_noiseModel_Gaussian")
BOOST_CLASS_EXPORT_GUID(gtsam::noiseModel::Unit, "gtsam_noiseModel_Unit"); BOOST_CLASS_EXPORT_GUID(gtsam::noiseModel::Unit, "gtsam_noiseModel_Unit")
BOOST_CLASS_EXPORT_GUID(gtsam::noiseModel::Isotropic, "gtsam_noiseModel_Isotropic"); BOOST_CLASS_EXPORT_GUID(gtsam::noiseModel::Isotropic, "gtsam_noiseModel_Isotropic")
BOOST_CLASS_EXPORT_GUID(gtsam::SharedNoiseModel, "gtsam_SharedNoiseModel"); BOOST_CLASS_EXPORT_GUID(gtsam::SharedNoiseModel, "gtsam_SharedNoiseModel")
BOOST_CLASS_EXPORT_GUID(gtsam::SharedDiagonal, "gtsam_SharedDiagonal"); BOOST_CLASS_EXPORT_GUID(gtsam::SharedDiagonal, "gtsam_SharedDiagonal")
/* ************************************************************************* */ /* ************************************************************************* */
// example noise models // example noise models
@ -129,9 +129,9 @@ TEST (Serialization, SharedDiagonal_noiseModels) {
/* Create GUIDs for factors */ /* Create GUIDs for factors */
/* ************************************************************************* */ /* ************************************************************************* */
BOOST_CLASS_EXPORT_GUID(gtsam::JacobianFactor, "gtsam::JacobianFactor"); BOOST_CLASS_EXPORT_GUID(gtsam::JacobianFactor, "gtsam::JacobianFactor")
BOOST_CLASS_EXPORT_GUID(gtsam::HessianFactor , "gtsam::HessianFactor"); BOOST_CLASS_EXPORT_GUID(gtsam::HessianFactor , "gtsam::HessianFactor")
BOOST_CLASS_EXPORT_GUID(gtsam::GaussianConditional , "gtsam::GaussianConditional"); BOOST_CLASS_EXPORT_GUID(gtsam::GaussianConditional , "gtsam::GaussianConditional")
/* ************************************************************************* */ /* ************************************************************************* */
TEST (Serialization, linear_factors) { TEST (Serialization, linear_factors) {

2
gtsam/navigation/CombinedImuFactor.cpp
View File

@ -257,5 +257,5 @@ std::ostream& operator<<(std::ostream& os, const CombinedImuFactor& f) {
/// namespace gtsam /// namespace gtsam
/// Boost serialization export definition for derived class /// Boost serialization export definition for derived class
BOOST_CLASS_EXPORT_IMPLEMENT(gtsam::PreintegrationCombinedParams); BOOST_CLASS_EXPORT_IMPLEMENT(gtsam::PreintegrationCombinedParams)

2
gtsam/navigation/CombinedImuFactor.h
View File

@ -353,4 +353,4 @@ struct traits<CombinedImuFactor> : public Testable<CombinedImuFactor> {};
} // namespace gtsam } // namespace gtsam
/// Add Boost serialization export key (declaration) for derived class /// Add Boost serialization export key (declaration) for derived class
BOOST_CLASS_EXPORT_KEY(gtsam::PreintegrationCombinedParams); BOOST_CLASS_EXPORT_KEY(gtsam::PreintegrationCombinedParams)

26
gtsam/navigation/navigation.i
View File

@ -41,6 +41,12 @@ class ConstantBias {
Vector gyroscope() const; Vector gyroscope() const;
Vector correctAccelerometer(Vector measurement) const; Vector correctAccelerometer(Vector measurement) const;
Vector correctGyroscope(Vector measurement) const; Vector correctGyroscope(Vector measurement) const;
// enabling serialization functionality
void serialize() const;
// enable pickling in python
void pickle() const;
}; };
}///\namespace imuBias }///\namespace imuBias
@ -64,6 +70,12 @@ class NavState {
gtsam::NavState retract(const Vector& x) const; gtsam::NavState retract(const Vector& x) const;
Vector localCoordinates(const gtsam::NavState& g) const; Vector localCoordinates(const gtsam::NavState& g) const;
// enabling serialization functionality
void serialize() const;
// enable pickling in python
void pickle() const;
}; };
#include <gtsam/navigation/PreintegratedRotation.h> #include <gtsam/navigation/PreintegratedRotation.h>
@ -88,6 +100,8 @@ virtual class PreintegratedRotationParams {
virtual class PreintegrationParams : gtsam::PreintegratedRotationParams { virtual class PreintegrationParams : gtsam::PreintegratedRotationParams {
PreintegrationParams(Vector n_gravity); PreintegrationParams(Vector n_gravity);
gtsam::Vector n_gravity;
static gtsam::PreintegrationParams* MakeSharedD(double g); static gtsam::PreintegrationParams* MakeSharedD(double g);
static gtsam::PreintegrationParams* MakeSharedU(double g); static gtsam::PreintegrationParams* MakeSharedU(double g);
static gtsam::PreintegrationParams* MakeSharedD(); // default g = 9.81 static gtsam::PreintegrationParams* MakeSharedD(); // default g = 9.81
@ -104,6 +118,12 @@ virtual class PreintegrationParams : gtsam::PreintegratedRotationParams {
Matrix getAccelerometerCovariance() const; Matrix getAccelerometerCovariance() const;
Matrix getIntegrationCovariance() const; Matrix getIntegrationCovariance() const;
bool getUse2ndOrderCoriolis() const; bool getUse2ndOrderCoriolis() const;
// enabling serialization functionality
void serialize() const;
// enable pickling in python
void pickle() const;
}; };
#include <gtsam/navigation/ImuFactor.h> #include <gtsam/navigation/ImuFactor.h>
@ -133,6 +153,12 @@ class PreintegratedImuMeasurements {
Vector biasHatVector() const; Vector biasHatVector() const;
gtsam::NavState predict(const gtsam::NavState& state_i, gtsam::NavState predict(const gtsam::NavState& state_i,
const gtsam::imuBias::ConstantBias& bias) const; const gtsam::imuBias::ConstantBias& bias) const;
// enabling serialization functionality
void serialize() const;
// enable pickling in python
void pickle() const;
}; };
virtual class ImuFactor: gtsam::NonlinearFactor { virtual class ImuFactor: gtsam::NonlinearFactor {

2
gtsam/navigation/tests/testAttitudeFactor.cpp
View File

@ -66,7 +66,7 @@ TEST( Rot3AttitudeFactor, Constructor ) {
/* ************************************************************************* */ /* ************************************************************************* */
// Export Noisemodels // Export Noisemodels
// See http://www.boost.org/doc/libs/1_32_0/libs/serialization/doc/special.html // See http://www.boost.org/doc/libs/1_32_0/libs/serialization/doc/special.html
BOOST_CLASS_EXPORT(gtsam::noiseModel::Isotropic); BOOST_CLASS_EXPORT(gtsam::noiseModel::Isotropic)
/* ************************************************************************* */ /* ************************************************************************* */
TEST(Rot3AttitudeFactor, Serialization) { TEST(Rot3AttitudeFactor, Serialization) {

4
gtsam/navigation/tests/testGPSFactor.cpp
View File

@ -72,7 +72,7 @@ TEST( GPSFactor, Constructor ) {
// Calculate numerical derivatives // Calculate numerical derivatives
Matrix expectedH = numericalDerivative11<Vector,Pose3>( Matrix expectedH = numericalDerivative11<Vector,Pose3>(
std::bind(&GPSFactor::evaluateError, &factor, _1, boost::none), T); std::bind(&GPSFactor::evaluateError, &factor, std::placeholders::_1, boost::none), T);
// Use the factor to calculate the derivative // Use the factor to calculate the derivative
Matrix actualH; Matrix actualH;
@ -101,7 +101,7 @@ TEST( GPSFactor2, Constructor ) {
// Calculate numerical derivatives // Calculate numerical derivatives
Matrix expectedH = numericalDerivative11<Vector,NavState>( Matrix expectedH = numericalDerivative11<Vector,NavState>(
std::bind(&GPSFactor2::evaluateError, &factor, _1, boost::none), T); std::bind(&GPSFactor2::evaluateError, &factor, std::placeholders::_1, boost::none), T);
// Use the factor to calculate the derivative // Use the factor to calculate the derivative
Matrix actualH; Matrix actualH;

14
gtsam/navigation/tests/testImuFactorSerialization.cpp
View File

@ -31,16 +31,16 @@ using namespace gtsam;
using namespace gtsam::serializationTestHelpers; using namespace gtsam::serializationTestHelpers;
BOOST_CLASS_EXPORT_GUID(gtsam::noiseModel::Constrained, BOOST_CLASS_EXPORT_GUID(gtsam::noiseModel::Constrained,
"gtsam_noiseModel_Constrained"); "gtsam_noiseModel_Constrained")
BOOST_CLASS_EXPORT_GUID(gtsam::noiseModel::Diagonal, BOOST_CLASS_EXPORT_GUID(gtsam::noiseModel::Diagonal,
"gtsam_noiseModel_Diagonal"); "gtsam_noiseModel_Diagonal")
BOOST_CLASS_EXPORT_GUID(gtsam::noiseModel::Gaussian, BOOST_CLASS_EXPORT_GUID(gtsam::noiseModel::Gaussian,
"gtsam_noiseModel_Gaussian"); "gtsam_noiseModel_Gaussian")
BOOST_CLASS_EXPORT_GUID(gtsam::noiseModel::Unit, "gtsam_noiseModel_Unit"); BOOST_CLASS_EXPORT_GUID(gtsam::noiseModel::Unit, "gtsam_noiseModel_Unit")
BOOST_CLASS_EXPORT_GUID(gtsam::noiseModel::Isotropic, BOOST_CLASS_EXPORT_GUID(gtsam::noiseModel::Isotropic,
"gtsam_noiseModel_Isotropic"); "gtsam_noiseModel_Isotropic")
BOOST_CLASS_EXPORT_GUID(gtsam::SharedNoiseModel, "gtsam_SharedNoiseModel"); BOOST_CLASS_EXPORT_GUID(gtsam::SharedNoiseModel, "gtsam_SharedNoiseModel")
BOOST_CLASS_EXPORT_GUID(gtsam::SharedDiagonal, "gtsam_SharedDiagonal"); BOOST_CLASS_EXPORT_GUID(gtsam::SharedDiagonal, "gtsam_SharedDiagonal")
template <typename P> template <typename P>
P getPreintegratedMeasurements() { P getPreintegratedMeasurements() {

16
gtsam/navigation/tests/testMagFactor.cpp
View File

@ -64,7 +64,7 @@ TEST( MagFactor, unrotate ) {
Point3 expected(22735.5, 314.502, 44202.5); Point3 expected(22735.5, 314.502, 44202.5);
EXPECT( assert_equal(expected, MagFactor::unrotate(theta,nM,H),1e-1)); EXPECT( assert_equal(expected, MagFactor::unrotate(theta,nM,H),1e-1));
EXPECT( assert_equal(numericalDerivative11<Point3,Rot2> // EXPECT( assert_equal(numericalDerivative11<Point3,Rot2> //
(std::bind(&MagFactor::unrotate, _1, nM, none), theta), H, 1e-6)); (std::bind(&MagFactor::unrotate, std::placeholders::_1, nM, none), theta), H, 1e-6));
} }
// ************************************************************************* // *************************************************************************
@ -76,35 +76,35 @@ TEST( MagFactor, Factors ) {
MagFactor f(1, measured, s, dir, bias, model); MagFactor f(1, measured, s, dir, bias, model);
EXPECT( assert_equal(Z_3x1,f.evaluateError(theta,H1),1e-5)); EXPECT( assert_equal(Z_3x1,f.evaluateError(theta,H1),1e-5));
EXPECT( assert_equal((Matrix)numericalDerivative11<Vector,Rot2> // EXPECT( assert_equal((Matrix)numericalDerivative11<Vector,Rot2> //
(std::bind(&MagFactor::evaluateError, &f, _1, none), theta), H1, 1e-7)); (std::bind(&MagFactor::evaluateError, &f, std::placeholders::_1, none), theta), H1, 1e-7));
// MagFactor1 // MagFactor1
MagFactor1 f1(1, measured, s, dir, bias, model); MagFactor1 f1(1, measured, s, dir, bias, model);
EXPECT( assert_equal(Z_3x1,f1.evaluateError(nRb,H1),1e-5)); EXPECT( assert_equal(Z_3x1,f1.evaluateError(nRb,H1),1e-5));
EXPECT( assert_equal(numericalDerivative11<Vector,Rot3> // EXPECT( assert_equal(numericalDerivative11<Vector,Rot3> //
(std::bind(&MagFactor1::evaluateError, &f1, _1, none), nRb), H1, 1e-7)); (std::bind(&MagFactor1::evaluateError, &f1, std::placeholders::_1, none), nRb), H1, 1e-7));
// MagFactor2 // MagFactor2
MagFactor2 f2(1, 2, measured, nRb, model); MagFactor2 f2(1, 2, measured, nRb, model);
EXPECT( assert_equal(Z_3x1,f2.evaluateError(scaled,bias,H1,H2),1e-5)); EXPECT( assert_equal(Z_3x1,f2.evaluateError(scaled,bias,H1,H2),1e-5));
EXPECT( assert_equal(numericalDerivative11<Vector,Point3> // EXPECT( assert_equal(numericalDerivative11<Vector,Point3> //
(std::bind(&MagFactor2::evaluateError, &f2, _1, bias, none, none), scaled),// (std::bind(&MagFactor2::evaluateError, &f2, std::placeholders::_1, bias, none, none), scaled),//
H1, 1e-7)); H1, 1e-7));
EXPECT( assert_equal(numericalDerivative11<Vector,Point3> // EXPECT( assert_equal(numericalDerivative11<Vector,Point3> //
(std::bind(&MagFactor2::evaluateError, &f2, scaled, _1, none, none), bias),// (std::bind(&MagFactor2::evaluateError, &f2, scaled, std::placeholders::_1, none, none), bias),//
H2, 1e-7)); H2, 1e-7));
// MagFactor2 // MagFactor2
MagFactor3 f3(1, 2, 3, measured, nRb, model); MagFactor3 f3(1, 2, 3, measured, nRb, model);
EXPECT(assert_equal(Z_3x1,f3.evaluateError(s,dir,bias,H1,H2,H3),1e-5)); EXPECT(assert_equal(Z_3x1,f3.evaluateError(s,dir,bias,H1,H2,H3),1e-5));
EXPECT(assert_equal((Matrix)numericalDerivative11<Vector,double> // EXPECT(assert_equal((Matrix)numericalDerivative11<Vector,double> //
(std::bind(&MagFactor3::evaluateError, &f3, _1, dir, bias, none, none, none), s),// (std::bind(&MagFactor3::evaluateError, &f3, std::placeholders::_1, dir, bias, none, none, none), s),//
H1, 1e-7)); H1, 1e-7));
EXPECT(assert_equal(numericalDerivative11<Vector,Unit3> // EXPECT(assert_equal(numericalDerivative11<Vector,Unit3> //
(std::bind(&MagFactor3::evaluateError, &f3, s, _1, bias, none, none, none), dir),// (std::bind(&MagFactor3::evaluateError, &f3, s, std::placeholders::_1, bias, none, none, none), dir),//
H2, 1e-7)); H2, 1e-7));
EXPECT(assert_equal(numericalDerivative11<Vector,Point3> // EXPECT(assert_equal(numericalDerivative11<Vector,Point3> //
(std::bind(&MagFactor3::evaluateError, &f3, s, dir, _1, none, none, none), bias),// (std::bind(&MagFactor3::evaluateError, &f3, s, dir, std::placeholders::_1, none, none, none), bias),//
H3, 1e-7)); H3, 1e-7));
} }

2
gtsam/nonlinear/CustomFactor.h
View File

@ -101,4 +101,4 @@ private:
} }
}; };
}; }

3
gtsam/nonlinear/DoglegOptimizerImpl.cpp
View File

@ -78,7 +78,8 @@ VectorValues DoglegOptimizerImpl::ComputeBlend(double delta, const VectorValues&
// Compute blended point // Compute blended point
if(verbose) cout << "In blend region with fraction " << tau << " of Newton's method point" << endl; if(verbose) cout << "In blend region with fraction " << tau << " of Newton's method point" << endl;
VectorValues blend = (1. - tau) * x_u; axpy(tau, x_n, blend); VectorValues blend = (1. - tau) * x_u;
blend += tau * x_n;
return blend; return blend;
} }

12
gtsam/nonlinear/Expression-inl.h
View File

@ -246,6 +246,18 @@ struct apply_compose {
return x.compose(y, H1, H2); return x.compose(y, H1, H2);
} }
}; };
template <>
struct apply_compose<double> {
double operator()(const double& x, const double& y,
OptionalJacobian<1, 1> H1 = boost::none,
OptionalJacobian<1, 1> H2 = boost::none) const {
if (H1) H1->setConstant(y);
if (H2) H2->setConstant(x);
return x * y;
}
};
} }
// Global methods: // Global methods:

2
gtsam/nonlinear/ExpressionFactor.h
View File

@ -305,7 +305,7 @@ struct traits<ExpressionFactorN<T, Args...>>
* \deprecated Prefer the more general ExpressionFactorN<>. * \deprecated Prefer the more general ExpressionFactorN<>.
*/ */
template <typename T, typename A1, typename A2> template <typename T, typename A1, typename A2>
class ExpressionFactor2 : public ExpressionFactorN<T, A1, A2> { class GTSAM_DEPRECATED ExpressionFactor2 : public ExpressionFactorN<T, A1, A2> {
public: public:
/// Destructor /// Destructor
~ExpressionFactor2() override {} ~ExpressionFactor2() override {}

5
gtsam/nonlinear/GncOptimizer.h
View File

@ -142,8 +142,9 @@ class GTSAM_EXPORT GncOptimizer {
* provides an extra interface for the user to initialize the weightst * provides an extra interface for the user to initialize the weightst
* */ * */
void setWeights(const Vector w) { void setWeights(const Vector w) {
if(w.size() != nfg_.size()){ if (size_t(w.size()) != nfg_.size()) {
throw std::runtime_error("GncOptimizer::setWeights: the number of specified weights" throw std::runtime_error(
"GncOptimizer::setWeights: the number of specified weights"
" does not match the size of the factor graph."); " does not match the size of the factor graph.");
} }
weights_ = w; weights_ = w;

6
gtsam/nonlinear/LinearContainerFactor.h
View File

@ -29,9 +29,6 @@ protected:
GaussianFactor::shared_ptr factor_; GaussianFactor::shared_ptr factor_;
boost::optional<Values> linearizationPoint_; boost::optional<Values> linearizationPoint_;
/** Default constructor - necessary for serialization */
LinearContainerFactor() {}
/** direct copy constructor */ /** direct copy constructor */
GTSAM_EXPORT LinearContainerFactor(const GaussianFactor::shared_ptr& factor, const boost::optional<Values>& linearizationPoint); GTSAM_EXPORT LinearContainerFactor(const GaussianFactor::shared_ptr& factor, const boost::optional<Values>& linearizationPoint);
@ -43,6 +40,9 @@ public:
typedef boost::shared_ptr<This> shared_ptr; typedef boost::shared_ptr<This> shared_ptr;
/** Default constructor - necessary for serialization */
LinearContainerFactor() {}
/** Primary constructor: store a linear factor with optional linearization point */ /** Primary constructor: store a linear factor with optional linearization point */
GTSAM_EXPORT LinearContainerFactor(const JacobianFactor& factor, const Values& linearizationPoint = Values()); GTSAM_EXPORT LinearContainerFactor(const JacobianFactor& factor, const Values& linearizationPoint = Values());

12
gtsam/nonlinear/Marginals.cpp
View File

@ -80,11 +80,15 @@ Marginals::Marginals(const GaussianFactorGraph& graph, const VectorValues& solut
/* ************************************************************************* */ /* ************************************************************************* */
void Marginals::computeBayesTree() { void Marginals::computeBayesTree() {
// The default ordering to use.
const Ordering::OrderingType defaultOrderingType = Ordering::COLAMD;
// Compute BayesTree // Compute BayesTree
if(factorization_ == CHOLESKY) if (factorization_ == CHOLESKY)
bayesTree_ = *graph_.eliminateMultifrontal(EliminatePreferCholesky); bayesTree_ = *graph_.eliminateMultifrontal(defaultOrderingType,
else if(factorization_ == QR) EliminatePreferCholesky);
bayesTree_ = *graph_.eliminateMultifrontal(EliminateQR); else if (factorization_ == QR)
bayesTree_ =
*graph_.eliminateMultifrontal(defaultOrderingType, EliminateQR);
} }
/* ************************************************************************* */ /* ************************************************************************* */

8
gtsam/nonlinear/Marginals.h
View File

@ -131,17 +131,19 @@ protected:
void computeBayesTree(const Ordering& ordering); void computeBayesTree(const Ordering& ordering);
public: public:
#ifdef GTSAM_ALLOW_DEPRECATED_SINCE_V41
/** \deprecated argument order changed due to removing boost::optional<Ordering> */ /** \deprecated argument order changed due to removing boost::optional<Ordering> */
Marginals(const NonlinearFactorGraph& graph, const Values& solution, Factorization factorization, GTSAM_DEPRECATED Marginals(const NonlinearFactorGraph& graph, const Values& solution, Factorization factorization,
const Ordering& ordering) : Marginals(graph, solution, ordering, factorization) {} const Ordering& ordering) : Marginals(graph, solution, ordering, factorization) {}
/** \deprecated argument order changed due to removing boost::optional<Ordering> */ /** \deprecated argument order changed due to removing boost::optional<Ordering> */
Marginals(const GaussianFactorGraph& graph, const Values& solution, Factorization factorization, GTSAM_DEPRECATED Marginals(const GaussianFactorGraph& graph, const Values& solution, Factorization factorization,
const Ordering& ordering) : Marginals(graph, solution, ordering, factorization) {} const Ordering& ordering) : Marginals(graph, solution, ordering, factorization) {}
/** \deprecated argument order changed due to removing boost::optional<Ordering> */ /** \deprecated argument order changed due to removing boost::optional<Ordering> */
Marginals(const GaussianFactorGraph& graph, const VectorValues& solution, Factorization factorization, GTSAM_DEPRECATED Marginals(const GaussianFactorGraph& graph, const VectorValues& solution, Factorization factorization,
const Ordering& ordering) : Marginals(graph, solution, ordering, factorization) {} const Ordering& ordering) : Marginals(graph, solution, ordering, factorization) {}
#endif
}; };

2
gtsam/nonlinear/NonlinearConjugateGradientOptimizer.h
View File

@ -149,7 +149,7 @@ boost::tuple<V, int> nonlinearConjugateGradient(const S &system,
const V &initial, const NonlinearOptimizerParams &params, const V &initial, const NonlinearOptimizerParams &params,
const bool singleIteration, const bool gradientDescent = false) { const bool singleIteration, const bool gradientDescent = false) {
// GTSAM_CONCEPT_MANIFOLD_TYPE(V); // GTSAM_CONCEPT_MANIFOLD_TYPE(V)
size_t iteration = 0; size_t iteration = 0;

1
gtsam/nonlinear/NonlinearEquality.h
View File

@ -219,7 +219,6 @@ protected:
X value_; /// fixed value for variable X value_; /// fixed value for variable
GTSAM_CONCEPT_MANIFOLD_TYPE(X) GTSAM_CONCEPT_MANIFOLD_TYPE(X)
GTSAM_CONCEPT_TESTABLE_TYPE(X) GTSAM_CONCEPT_TESTABLE_TYPE(X)
public: public:

2
gtsam/nonlinear/NonlinearFactor.cpp
View File

@ -114,7 +114,7 @@ double NoiseModelFactor::weight(const Values& c) const {
if (noiseModel_) { if (noiseModel_) {
const Vector b = unwhitenedError(c); const Vector b = unwhitenedError(c);
check(noiseModel_, b.size()); check(noiseModel_, b.size());
return 0.5 * noiseModel_->weight(b); return noiseModel_->weight(b);
} }
else else
return 1.0; return 1.0;

6
gtsam/nonlinear/NonlinearFactorGraph.h
View File

@ -265,15 +265,17 @@ namespace gtsam {
public: public:
#ifdef GTSAM_ALLOW_DEPRECATED_SINCE_V41
/** \deprecated */ /** \deprecated */
boost::shared_ptr<HessianFactor> linearizeToHessianFactor( boost::shared_ptr<HessianFactor> GTSAM_DEPRECATED linearizeToHessianFactor(
const Values& values, boost::none_t, const Dampen& dampen = nullptr) const const Values& values, boost::none_t, const Dampen& dampen = nullptr) const
{return linearizeToHessianFactor(values, dampen);} {return linearizeToHessianFactor(values, dampen);}
/** \deprecated */ /** \deprecated */
Values updateCholesky(const Values& values, boost::none_t, Values GTSAM_DEPRECATED updateCholesky(const Values& values, boost::none_t,
const Dampen& dampen = nullptr) const const Dampen& dampen = nullptr) const
{return updateCholesky(values, dampen);} {return updateCholesky(values, dampen);}
#endif
}; };

10
gtsam/nonlinear/NonlinearOptimizer.cpp
View File

@ -147,11 +147,13 @@ VectorValues NonlinearOptimizer::solve(const GaussianFactorGraph& gfg,
} else if (params.isSequential()) { } else if (params.isSequential()) {
// Sequential QR or Cholesky (decided by params.getEliminationFunction()) // Sequential QR or Cholesky (decided by params.getEliminationFunction())
if (params.ordering) if (params.ordering)
delta = gfg.eliminateSequential(*params.ordering, params.getEliminationFunction(), delta = gfg.eliminateSequential(*params.ordering,
boost::none, params.orderingType)->optimize(); params.getEliminationFunction())
->optimize();
else else
delta = gfg.eliminateSequential(params.getEliminationFunction(), boost::none, delta = gfg.eliminateSequential(params.orderingType,
params.orderingType)->optimize(); params.getEliminationFunction())
->optimize();
} else if (params.isIterative()) { } else if (params.isIterative()) {
// Conjugate Gradient -> needs params.iterativeParams // Conjugate Gradient -> needs params.iterativeParams
if (!params.iterativeParams) if (!params.iterativeParams)

12
gtsam/nonlinear/Values.h
View File

@ -24,6 +24,7 @@
#pragma once #pragma once
#include <gtsam/base/FastDefaultAllocator.h>
#include <gtsam/base/GenericValue.h> #include <gtsam/base/GenericValue.h>
#include <gtsam/base/VectorSpace.h> #include <gtsam/base/VectorSpace.h>
#include <gtsam/inference/Key.h> #include <gtsam/inference/Key.h>
@ -62,17 +63,18 @@ namespace gtsam {
class GTSAM_EXPORT Values { class GTSAM_EXPORT Values {
private: private:
// Internally we store a boost ptr_map, with a ValueCloneAllocator (defined // Internally we store a boost ptr_map, with a ValueCloneAllocator (defined
// below) to clone and deallocate the Value objects, and a boost // below) to clone and deallocate the Value objects, and our compile-flag-
// fast_pool_allocator to allocate map nodes. In this way, all memory is // dependent FastDefaultAllocator to allocate map nodes. In this way, the
// allocated in a boost memory pool. // user defines the allocation details (i.e. optimize for memory pool/arenas
// concurrency).
typedef internal::FastDefaultAllocator<typename std::pair<const Key, void*>>::type KeyValuePtrPairAllocator;
typedef boost::ptr_map< typedef boost::ptr_map<
Key, Key,
Value, Value,
std::less<Key>, std::less<Key>,
ValueCloneAllocator, ValueCloneAllocator,
boost::fast_pool_allocator<std::pair<const Key, void*> > > KeyValueMap; KeyValuePtrPairAllocator > KeyValueMap;
// The member to store the values, see just above // The member to store the values, see just above
KeyValueMap values_; KeyValueMap values_;

11
gtsam/nonlinear/factorTesting.h
View File

@ -35,8 +35,7 @@ namespace gtsam {
* zero errors anyway. However, it means that below will only be exact for the correct measurement. * zero errors anyway. However, it means that below will only be exact for the correct measurement.
*/ */
JacobianFactor linearizeNumerically(const NoiseModelFactor& factor, JacobianFactor linearizeNumerically(const NoiseModelFactor& factor,
const Values& values, double delta = 1e-5) { const Values& values, double delta = 1e-5) {
// We will fill a vector of key/Jacobians pairs (a map would sort) // We will fill a vector of key/Jacobians pairs (a map would sort)
std::vector<std::pair<Key, Matrix> > jacobians; std::vector<std::pair<Key, Matrix> > jacobians;
@ -46,24 +45,24 @@ JacobianFactor linearizeNumerically(const NoiseModelFactor& factor,
// Loop over all variables // Loop over all variables
const double one_over_2delta = 1.0 / (2.0 * delta); const double one_over_2delta = 1.0 / (2.0 * delta);
for(Key key: factor) { for (Key key : factor) {
// Compute central differences using the values struct. // Compute central differences using the values struct.
VectorValues dX = values.zeroVectors(); VectorValues dX = values.zeroVectors();
const size_t cols = dX.dim(key); const size_t cols = dX.dim(key);
Matrix J = Matrix::Zero(rows, cols); Matrix J = Matrix::Zero(rows, cols);
for (size_t col = 0; col < cols; ++col) { for (size_t col = 0; col < cols; ++col) {
Eigen::VectorXd dx = Eigen::VectorXd::Zero(cols); Eigen::VectorXd dx = Eigen::VectorXd::Zero(cols);
dx[col] = delta; dx(col) = delta;
dX[key] = dx; dX[key] = dx;
Values eval_values = values.retract(dX); Values eval_values = values.retract(dX);
const Eigen::VectorXd left = factor.whitenedError(eval_values); const Eigen::VectorXd left = factor.whitenedError(eval_values);
dx[col] = -delta; dx(col) = -delta;
dX[key] = dx; dX[key] = dx;
eval_values = values.retract(dX); eval_values = values.retract(dX);
const Eigen::VectorXd right = factor.whitenedError(eval_values); const Eigen::VectorXd right = factor.whitenedError(eval_values);
J.col(col) = (left - right) * one_over_2delta; J.col(col) = (left - right) * one_over_2delta;
} }
jacobians.push_back(std::make_pair(key,J)); jacobians.push_back(std::make_pair(key, J));
} }
// Next step...return JacobianFactor // Next step...return JacobianFactor

16
gtsam/nonlinear/nonlinear.i
View File

@ -115,6 +115,10 @@ class Ordering {
Ordering(); Ordering();
Ordering(const gtsam::Ordering& other); Ordering(const gtsam::Ordering& other);
template <FACTOR_GRAPH = {gtsam::NonlinearFactorGraph,
gtsam::GaussianFactorGraph}>
static gtsam::Ordering Colamd(const FACTOR_GRAPH& graph);
// Testable // Testable
void print(string s = "", const gtsam::KeyFormatter& keyFormatter = void print(string s = "", const gtsam::KeyFormatter& keyFormatter =
gtsam::DefaultKeyFormatter) const; gtsam::DefaultKeyFormatter) const;
@ -734,7 +738,12 @@ class ISAM2 {
const gtsam::KeyList& extraReelimKeys, const gtsam::KeyList& extraReelimKeys,
bool force_relinearize); bool force_relinearize);
gtsam::ISAM2Result update(const gtsam::NonlinearFactorGraph& newFactors,
const gtsam::Values& newTheta,
const gtsam::ISAM2UpdateParams& updateParams);
gtsam::Values getLinearizationPoint() const; gtsam::Values getLinearizationPoint() const;
bool valueExists(gtsam::Key key) const;
gtsam::Values calculateEstimate() const; gtsam::Values calculateEstimate() const;
template <VALUE = {gtsam::Point2, gtsam::Rot2, gtsam::Pose2, gtsam::Point3, template <VALUE = {gtsam::Point2, gtsam::Rot2, gtsam::Pose2, gtsam::Point3,
gtsam::Rot3, gtsam::Pose3, gtsam::Cal3_S2, gtsam::Cal3DS2, gtsam::Rot3, gtsam::Pose3, gtsam::Cal3_S2, gtsam::Cal3DS2,
@ -744,12 +753,17 @@ class ISAM2 {
gtsam::PinholeCamera<gtsam::Cal3Fisheye>, gtsam::PinholeCamera<gtsam::Cal3Fisheye>,
gtsam::PinholeCamera<gtsam::Cal3Unified>, Vector, Matrix}> gtsam::PinholeCamera<gtsam::Cal3Unified>, Vector, Matrix}>
VALUE calculateEstimate(size_t key) const; VALUE calculateEstimate(size_t key) const;
gtsam::Values calculateBestEstimate() const;
Matrix marginalCovariance(size_t key) const; Matrix marginalCovariance(size_t key) const;
gtsam::Values calculateBestEstimate() const;
gtsam::VectorValues getDelta() const; gtsam::VectorValues getDelta() const;
double error(const gtsam::VectorValues& x) const;
gtsam::NonlinearFactorGraph getFactorsUnsafe() const; gtsam::NonlinearFactorGraph getFactorsUnsafe() const;
gtsam::VariableIndex getVariableIndex() const; gtsam::VariableIndex getVariableIndex() const;
const gtsam::KeySet& getFixedVariables() const;
gtsam::ISAM2Params params() const; gtsam::ISAM2Params params() const;
void printStats() const;
gtsam::VectorValues gradientAtZero() const;
}; };
#include <gtsam/nonlinear/NonlinearISAM.h> #include <gtsam/nonlinear/NonlinearISAM.h>

13
gtsam/nonlinear/tests/testExpression.cpp
View File

@ -293,6 +293,19 @@ TEST(Expression, compose3) {
EXPECT(expected == R3.keys()); EXPECT(expected == R3.keys());
} }
/* ************************************************************************* */
// Test compose with double type (should be multiplication).
TEST(Expression, compose4) {
// Create expression
gtsam::Key key = 1;
Double_ R1(key), R2(key);
Double_ R3 = R1 * R2;
// Check keys
set<Key> expected = list_of(1);
EXPECT(expected == R3.keys());
}
/* ************************************************************************* */ /* ************************************************************************* */
// Test with ternary function. // Test with ternary function.
Rot3 composeThree(const Rot3& R1, const Rot3& R2, const Rot3& R3, OptionalJacobian<3, 3> H1, Rot3 composeThree(const Rot3& R1, const Rot3& R2, const Rot3& R3, OptionalJacobian<3, 3> H1,

48
gtsam/nonlinear/tests/testSerializationNonlinear.cpp
View File

@ -35,37 +35,37 @@ using namespace gtsam::serializationTestHelpers;
/* ************************************************************************* */ /* ************************************************************************* */
// Create GUIDs for Noisemodels // Create GUIDs for Noisemodels
BOOST_CLASS_EXPORT_GUID(gtsam::noiseModel::Diagonal, "gtsam_noiseModel_Diagonal"); BOOST_CLASS_EXPORT_GUID(gtsam::noiseModel::Diagonal, "gtsam_noiseModel_Diagonal")
BOOST_CLASS_EXPORT_GUID(gtsam::noiseModel::mEstimator::Base , "gtsam_noiseModel_mEstimator_Base"); BOOST_CLASS_EXPORT_GUID(gtsam::noiseModel::mEstimator::Base , "gtsam_noiseModel_mEstimator_Base")
BOOST_CLASS_EXPORT_GUID(gtsam::noiseModel::mEstimator::Null , "gtsam_noiseModel_mEstimator_Null"); BOOST_CLASS_EXPORT_GUID(gtsam::noiseModel::mEstimator::Null , "gtsam_noiseModel_mEstimator_Null")
BOOST_CLASS_EXPORT_GUID(gtsam::noiseModel::mEstimator::Fair , "gtsam_noiseModel_mEstimator_Fair"); BOOST_CLASS_EXPORT_GUID(gtsam::noiseModel::mEstimator::Fair , "gtsam_noiseModel_mEstimator_Fair")
BOOST_CLASS_EXPORT_GUID(gtsam::noiseModel::mEstimator::Huber, "gtsam_noiseModel_mEstimator_Huber"); BOOST_CLASS_EXPORT_GUID(gtsam::noiseModel::mEstimator::Huber, "gtsam_noiseModel_mEstimator_Huber")
BOOST_CLASS_EXPORT_GUID(gtsam::noiseModel::mEstimator::Tukey, "gtsam_noiseModel_mEstimator_Tukey"); BOOST_CLASS_EXPORT_GUID(gtsam::noiseModel::mEstimator::Tukey, "gtsam_noiseModel_mEstimator_Tukey")
BOOST_CLASS_EXPORT_GUID(gtsam::noiseModel::Constrained, "gtsam_noiseModel_Constrained"); BOOST_CLASS_EXPORT_GUID(gtsam::noiseModel::Constrained, "gtsam_noiseModel_Constrained")
BOOST_CLASS_EXPORT_GUID(gtsam::noiseModel::Unit, "gtsam_noiseModel_Unit"); BOOST_CLASS_EXPORT_GUID(gtsam::noiseModel::Unit, "gtsam_noiseModel_Unit")
BOOST_CLASS_EXPORT_GUID(gtsam::noiseModel::Isotropic,"gtsam_noiseModel_Isotropic"); BOOST_CLASS_EXPORT_GUID(gtsam::noiseModel::Isotropic,"gtsam_noiseModel_Isotropic")
BOOST_CLASS_EXPORT_GUID(gtsam::noiseModel::Robust, "gtsam_noiseModel_Robust"); BOOST_CLASS_EXPORT_GUID(gtsam::noiseModel::Robust, "gtsam_noiseModel_Robust")
BOOST_CLASS_EXPORT_GUID(gtsam::SharedNoiseModel, "gtsam_SharedNoiseModel"); BOOST_CLASS_EXPORT_GUID(gtsam::SharedNoiseModel, "gtsam_SharedNoiseModel")
BOOST_CLASS_EXPORT_GUID(gtsam::SharedDiagonal, "gtsam_SharedDiagonal"); BOOST_CLASS_EXPORT_GUID(gtsam::SharedDiagonal, "gtsam_SharedDiagonal")
/* ************************************************************************* */ /* ************************************************************************* */
// Create GUIDs for factors // Create GUIDs for factors
BOOST_CLASS_EXPORT_GUID(gtsam::PriorFactor<gtsam::Pose3>, "gtsam::PriorFactor<gtsam::Pose3>"); BOOST_CLASS_EXPORT_GUID(gtsam::PriorFactor<gtsam::Pose3>, "gtsam::PriorFactor<gtsam::Pose3>")
BOOST_CLASS_EXPORT_GUID(gtsam::JacobianFactor, "gtsam::JacobianFactor"); BOOST_CLASS_EXPORT_GUID(gtsam::JacobianFactor, "gtsam::JacobianFactor")
BOOST_CLASS_EXPORT_GUID(gtsam::HessianFactor , "gtsam::HessianFactor"); BOOST_CLASS_EXPORT_GUID(gtsam::HessianFactor , "gtsam::HessianFactor")
BOOST_CLASS_EXPORT_GUID(gtsam::GaussianConditional , "gtsam::GaussianConditional"); BOOST_CLASS_EXPORT_GUID(gtsam::GaussianConditional , "gtsam::GaussianConditional")
/* ************************************************************************* */ /* ************************************************************************* */
// Export all classes derived from Value // Export all classes derived from Value
GTSAM_VALUE_EXPORT(gtsam::Cal3_S2); GTSAM_VALUE_EXPORT(gtsam::Cal3_S2)
GTSAM_VALUE_EXPORT(gtsam::Cal3Bundler); GTSAM_VALUE_EXPORT(gtsam::Cal3Bundler)
GTSAM_VALUE_EXPORT(gtsam::Point3); GTSAM_VALUE_EXPORT(gtsam::Point3)
GTSAM_VALUE_EXPORT(gtsam::Pose3); GTSAM_VALUE_EXPORT(gtsam::Pose3)
GTSAM_VALUE_EXPORT(gtsam::Rot3); GTSAM_VALUE_EXPORT(gtsam::Rot3)
GTSAM_VALUE_EXPORT(gtsam::PinholeCamera<Cal3_S2>); GTSAM_VALUE_EXPORT(gtsam::PinholeCamera<Cal3_S2>)
GTSAM_VALUE_EXPORT(gtsam::PinholeCamera<Cal3DS2>); GTSAM_VALUE_EXPORT(gtsam::PinholeCamera<Cal3DS2>)
GTSAM_VALUE_EXPORT(gtsam::PinholeCamera<Cal3Bundler>); GTSAM_VALUE_EXPORT(gtsam::PinholeCamera<Cal3Bundler>)
namespace detail { namespace detail {
template<class T> struct pack { template<class T> struct pack {

2
gtsam/slam/BetweenFactor.h
View File

@ -103,7 +103,7 @@ namespace gtsam {
boost::none, boost::optional<Matrix&> H2 = boost::none) const override { boost::none, boost::optional<Matrix&> H2 = boost::none) const override {
T hx = traits<T>::Between(p1, p2, H1, H2); // h(x) T hx = traits<T>::Between(p1, p2, H1, H2); // h(x)
// manifold equivalent of h(x)-z -> log(z,h(x)) // manifold equivalent of h(x)-z -> log(z,h(x))
#ifdef SLOW_BUT_CORRECT_BETWEENFACTOR #ifdef GTSAM_SLOW_BUT_CORRECT_BETWEENFACTOR
typename traits<T>::ChartJacobian::Jacobian Hlocal; typename traits<T>::ChartJacobian::Jacobian Hlocal;
Vector rval = traits<T>::Local(measured_, hx, boost::none, (H1 || H2) ? &Hlocal : 0); Vector rval = traits<T>::Local(measured_, hx, boost::none, (H1 || H2) ? &Hlocal : 0);
if (H1) *H1 = Hlocal * (*H1); if (H1) *H1 = Hlocal * (*H1);

6
gtsam/slam/GeneralSFMFactor.h
View File

@ -59,8 +59,8 @@ namespace gtsam {
template<class CAMERA, class LANDMARK> template<class CAMERA, class LANDMARK>
class GeneralSFMFactor: public NoiseModelFactor2<CAMERA, LANDMARK> { class GeneralSFMFactor: public NoiseModelFactor2<CAMERA, LANDMARK> {
GTSAM_CONCEPT_MANIFOLD_TYPE(CAMERA); GTSAM_CONCEPT_MANIFOLD_TYPE(CAMERA)
GTSAM_CONCEPT_MANIFOLD_TYPE(LANDMARK); GTSAM_CONCEPT_MANIFOLD_TYPE(LANDMARK)
static const int DimC = FixedDimension<CAMERA>::value; static const int DimC = FixedDimension<CAMERA>::value;
static const int DimL = FixedDimension<LANDMARK>::value; static const int DimL = FixedDimension<LANDMARK>::value;
@ -202,7 +202,7 @@ struct traits<GeneralSFMFactor<CAMERA, LANDMARK> > : Testable<
template<class CALIBRATION> template<class CALIBRATION>
class GeneralSFMFactor2: public NoiseModelFactor3<Pose3, Point3, CALIBRATION> { class GeneralSFMFactor2: public NoiseModelFactor3<Pose3, Point3, CALIBRATION> {
GTSAM_CONCEPT_MANIFOLD_TYPE(CALIBRATION); GTSAM_CONCEPT_MANIFOLD_TYPE(CALIBRATION)
static const int DimK = FixedDimension<CALIBRATION>::value; static const int DimK = FixedDimension<CALIBRATION>::value;
protected: protected:

55
gtsam/slam/JacobianFactorSVD.h
View File

@ -9,20 +9,21 @@
namespace gtsam { namespace gtsam {
/** /**
* JacobianFactor for Schur complement that uses the "Nullspace Trick" by Mourikis * JacobianFactor for Schur complement that uses the "Nullspace Trick" by
* Mourikis et al.
* *
* This trick is equivalent to the Schur complement, but can be faster. * This trick is equivalent to the Schur complement, but can be faster.
* In essence, the linear factor |E*dp + F*dX - b|, where p is point and X are poses, * In essence, the linear factor |E*dp + F*dX - b|, where p is point and X are
* is multiplied by Enull, a matrix that spans the left nullspace of E, i.e., * poses, is multiplied by Enull, a matrix that spans the left nullspace of E,
* The mx3 matrix is analyzed with SVD as E = [Erange Enull]*S*V (mxm * mx3 * 3x3) * i.e., The mx3 matrix is analyzed with SVD as E = [Erange Enull]*S*V (mxm *
* where Enull is an m x (m-3) matrix * mx3 * 3x3) where Enull is an m x (m-3) matrix Then Enull'*E*dp = 0, and
* Then Enull'*E*dp = 0, and
* |Enull'*E*dp + Enull'*F*dX - Enull'*b| == |Enull'*F*dX - Enull'*b| * |Enull'*E*dp + Enull'*F*dX - Enull'*b| == |Enull'*F*dX - Enull'*b|
* Normally F is m x 6*numKeys, and Enull'*F yields an (m-3) x 6*numKeys matrix. * Normally F is m x 6*numKeys, and Enull'*F yields an (m-3) x 6*numKeys matrix.
* *
* The code below assumes that F is block diagonal and is given as a vector of ZDim*D blocks. * The code below assumes that F is block diagonal and is given as a vector of
* Example: m = 4 (2 measurements), Enull = 4*1, F = 4*12 (for D=6) * ZDim*D blocks. Example: m = 4 (2 measurements), Enull = 4*1, F = 4*12 (for
* Then Enull'*F = 1*4 * 4*12 = 1*12, but each 1*6 piece can be computed as a 1x2 * 2x6 mult * D=6) Then Enull'*F = 1*4 * 4*12 = 1*12, but each 1*6 piece can be computed as
* a 1x2 * 2x6 multiplication.
*/ */
template<size_t D, size_t ZDim> template<size_t D, size_t ZDim>
class JacobianFactorSVD: public RegularJacobianFactor<D> { class JacobianFactorSVD: public RegularJacobianFactor<D> {
@ -37,10 +38,10 @@ public:
JacobianFactorSVD() { JacobianFactorSVD() {
} }
/// Empty constructor with keys /// Empty constructor with keys.
JacobianFactorSVD(const KeyVector& keys, // JacobianFactorSVD(const KeyVector& keys,
const SharedDiagonal& model = SharedDiagonal()) : const SharedDiagonal& model = SharedDiagonal())
Base() { : Base() {
Matrix zeroMatrix = Matrix::Zero(0, D); Matrix zeroMatrix = Matrix::Zero(0, D);
Vector zeroVector = Vector::Zero(0); Vector zeroVector = Vector::Zero(0);
std::vector<KeyMatrix> QF; std::vector<KeyMatrix> QF;
@ -51,18 +52,21 @@ public:
} }
/** /**
* @brief Constructor * @brief Construct a new JacobianFactorSVD object, createing a reduced-rank
* Takes the CameraSet derivatives (as ZDim*D blocks of block-diagonal F) * Jacobian factor on the CameraSet.
* and a reduced point derivative, Enull
* and creates a reduced-rank Jacobian factor on the CameraSet
* *
* @Fblocks: * @param keys keys associated with F blocks.
* @param Fblocks CameraSet derivatives, ZDim*D blocks of block-diagonal F
* @param Enull a reduced point derivative
* @param b right-hand side
* @param model noise model
*/ */
JacobianFactorSVD(const KeyVector& keys, JacobianFactorSVD(
const std::vector<MatrixZD, Eigen::aligned_allocator<MatrixZD> >& Fblocks, const Matrix& Enull, const KeyVector& keys,
const Vector& b, // const std::vector<MatrixZD, Eigen::aligned_allocator<MatrixZD> >& Fblocks,
const SharedDiagonal& model = SharedDiagonal()) : const Matrix& Enull, const Vector& b,
Base() { const SharedDiagonal& model = SharedDiagonal())
: Base() {
size_t numKeys = Enull.rows() / ZDim; size_t numKeys = Enull.rows() / ZDim;
size_t m2 = ZDim * numKeys - 3; // TODO: is this not just Enull.rows()? size_t m2 = ZDim * numKeys - 3; // TODO: is this not just Enull.rows()?
// PLAIN nullptr SPACE TRICK // PLAIN nullptr SPACE TRICK
@ -74,9 +78,8 @@ public:
QF.reserve(numKeys); QF.reserve(numKeys);
for (size_t k = 0; k < Fblocks.size(); ++k) { for (size_t k = 0; k < Fblocks.size(); ++k) {
Key key = keys[k]; Key key = keys[k];
QF.push_back( QF.emplace_back(
KeyMatrix(key, key, (Enull.transpose()).block(0, ZDim * k, m2, ZDim) * Fblocks[k]);
(Enull.transpose()).block(0, ZDim * k, m2, ZDim) * Fblocks[k]));
} }
JacobianFactor::fillTerms(QF, Enull.transpose() * b, model); JacobianFactor::fillTerms(QF, Enull.transpose() * b, model);
} }

Some files were not shown because too many files have changed in this diff Show More