Merge branch 'develop' of https://github.com/borglab/gtsam into feature/wrap_camVector
Sushmita
commit
2fab69e0e8
|
|
@ -1,51 +1,64 @@
|
|||
# Check / set dependent variables for MATLAB wrapper
|
||||
if(GTSAM_INSTALL_MATLAB_TOOLBOX)
|
||||
find_package(Matlab COMPONENTS MEX_COMPILER REQUIRED)
|
||||
if(NOT Matlab_MEX_COMPILER)
|
||||
message(FATAL_ERROR "Cannot find MEX compiler binary. Please check your Matlab installation and ensure MEX in installed as well.")
|
||||
endif()
|
||||
|
||||
if(GTSAM_BUILD_TYPE_POSTFIXES)
|
||||
set(CURRENT_POSTFIX ${CMAKE_${CMAKE_BUILD_TYPE_UPPER}_POSTFIX})
|
||||
endif()
|
||||
|
||||
if(NOT BUILD_SHARED_LIBS)
|
||||
message(FATAL_ERROR "GTSAM_INSTALL_MATLAB_TOOLBOX and BUILD_SHARED_LIBS=OFF. The MATLAB wrapper cannot be compiled with a static GTSAM library because mex modules are themselves shared libraries. If you want a self-contained mex module, enable GTSAM_MEX_BUILD_STATIC_MODULE instead of BUILD_SHARED_LIBS=OFF.")
|
||||
endif()
|
||||
endif()
|
||||
|
||||
# Set up cache options
|
||||
option(GTSAM_MEX_BUILD_STATIC_MODULE "Build MATLAB wrapper statically (increases build time)" OFF)
|
||||
set(GTSAM_BUILD_MEX_BINARY_FLAGS "" CACHE STRING "Extra flags for running Matlab MEX compilation")
|
||||
set(GTSAM_TOOLBOX_INSTALL_PATH "" CACHE PATH "Matlab toolbox destination, blank defaults to CMAKE_INSTALL_PREFIX/gtsam_toolbox")
|
||||
if(NOT GTSAM_TOOLBOX_INSTALL_PATH)
|
||||
set(GTSAM_TOOLBOX_INSTALL_PATH "${CMAKE_INSTALL_PREFIX}/gtsam_toolbox")
|
||||
set(GTSAM_TOOLBOX_INSTALL_PATH "${CMAKE_INSTALL_PREFIX}/gtsam_toolbox")
|
||||
endif()
|
||||
|
||||
# GTSAM_MEX_BUILD_STATIC_MODULE is not for Windows - on Windows any static
|
||||
# are already compiled into the library by the linker
|
||||
if(GTSAM_MEX_BUILD_STATIC_MODULE AND WIN32)
|
||||
message(FATAL_ERROR "GTSAM_MEX_BUILD_STATIC_MODULE should not be set on Windows - the linker already automatically compiles in any dependent static libraries. To create a standalone toolbox pacakge, simply ensure that CMake finds the static versions of all dependent libraries (Boost, etc).")
|
||||
message(FATAL_ERROR "GTSAM_MEX_BUILD_STATIC_MODULE should not be set on Windows - the linker already automatically compiles in any dependent static libraries. To create a standalone toolbox pacakge, simply ensure that CMake finds the static versions of all dependent libraries (Boost, etc).")
|
||||
endif()
|
||||
|
||||
# Try to automatically configure mex path
|
||||
if(APPLE)
|
||||
file(GLOB matlab_bin_directories "/Applications/MATLAB*/bin")
|
||||
set(mex_program_name "mex")
|
||||
elseif(WIN32)
|
||||
file(GLOB matlab_bin_directories "C:/Program Files*/MATLAB/*/bin")
|
||||
set(mex_program_name "mex.bat")
|
||||
else()
|
||||
file(GLOB matlab_bin_directories "/usr/local/MATLAB/*/bin")
|
||||
set(mex_program_name "mex")
|
||||
endif()
|
||||
set(MEX_COMMAND ${Matlab_MEX_COMPILER} CACHE PATH "Path to MATLAB MEX compiler")
|
||||
set(MATLAB_ROOT ${Matlab_ROOT_DIR} CACHE PATH "Path to MATLAB installation root (e.g. /usr/local/MATLAB/R2012a)")
|
||||
|
||||
# Try to automatically configure mex path from provided custom `bin` path.
|
||||
if(GTSAM_CUSTOM_MATLAB_PATH)
|
||||
set(matlab_bin_directories ${GTSAM_CUSTOM_MATLAB_PATH})
|
||||
endif()
|
||||
set(matlab_bin_directory ${GTSAM_CUSTOM_MATLAB_PATH})
|
||||
|
||||
# Run find_program explicitly putting $PATH after our predefined program
|
||||
# directories using 'ENV PATH' and 'NO_SYSTEM_ENVIRONMENT_PATH' - this prevents
|
||||
# finding the LaTeX mex program (totally unrelated to MATLAB Mex) when LaTeX is
|
||||
# on the system path.
|
||||
list(REVERSE matlab_bin_directories) # Reverse list so the highest version (sorted alphabetically) is preferred
|
||||
find_program(MEX_COMMAND ${mex_program_name}
|
||||
PATHS ${matlab_bin_directories} ENV PATH
|
||||
NO_DEFAULT_PATH)
|
||||
mark_as_advanced(FORCE MEX_COMMAND)
|
||||
# Now that we have mex, trace back to find the Matlab installation root
|
||||
get_filename_component(MEX_COMMAND "${MEX_COMMAND}" REALPATH)
|
||||
get_filename_component(mex_path "${MEX_COMMAND}" PATH)
|
||||
if(mex_path MATCHES ".*/win64$")
|
||||
get_filename_component(MATLAB_ROOT "${mex_path}/../.." ABSOLUTE)
|
||||
else()
|
||||
get_filename_component(MATLAB_ROOT "${mex_path}/.." ABSOLUTE)
|
||||
if(WIN32)
|
||||
set(mex_program_name "mex.bat")
|
||||
else()
|
||||
set(mex_program_name "mex")
|
||||
endif()
|
||||
|
||||
# Run find_program explicitly putting $PATH after our predefined program
|
||||
# directories using 'ENV PATH' and 'NO_SYSTEM_ENVIRONMENT_PATH' - this prevents
|
||||
# finding the LaTeX mex program (totally unrelated to MATLAB Mex) when LaTeX is
|
||||
# on the system path.
|
||||
find_program(MEX_COMMAND ${mex_program_name}
|
||||
PATHS ${matlab_bin_directory} ENV PATH
|
||||
NO_DEFAULT_PATH)
|
||||
|
||||
mark_as_advanced(FORCE MEX_COMMAND)
|
||||
# Now that we have mex, trace back to find the Matlab installation root
|
||||
get_filename_component(MEX_COMMAND "${MEX_COMMAND}" REALPATH)
|
||||
get_filename_component(mex_path "${MEX_COMMAND}" PATH)
|
||||
if(mex_path MATCHES ".*/win64$")
|
||||
get_filename_component(MATLAB_ROOT "${mex_path}/../.." ABSOLUTE)
|
||||
else()
|
||||
get_filename_component(MATLAB_ROOT "${mex_path}/.." ABSOLUTE)
|
||||
endif()
|
||||
endif()
|
||||
set(MATLAB_ROOT "${MATLAB_ROOT}" CACHE PATH "Path to MATLAB installation root (e.g. /usr/local/MATLAB/R2012a)")
|
||||
|
||||
|
||||
# User-friendly wrapping function. Builds a mex module from the provided
|
||||
|
|
|
|||
|
|
@ -24,6 +24,7 @@ option(GTSAM_WITH_EIGEN_MKL "Eigen will use Intel MKL if available"
|
|||
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_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_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_TANGENT_PREINTEGRATION "Use new ImuFactor with integration on tangent space" ON)
|
||||
|
|
@ -40,16 +41,5 @@ elseif(GTSAM_ROT3_EXPMAP)
|
|||
set(GTSAM_POSE3_EXPMAP 1 CACHE BOOL "" FORCE)
|
||||
endif()
|
||||
|
||||
# Options relating to MATLAB wrapper
|
||||
# TODO: Check for matlab mex binary before handling building of binaries
|
||||
option(GTSAM_INSTALL_MATLAB_TOOLBOX "Enable/Disable installation of matlab toolbox" OFF)
|
||||
# Set the default Python version. This is later updated in HandlePython.cmake.
|
||||
set(GTSAM_PYTHON_VERSION "Default" CACHE STRING "The version of Python to build the wrappers against.")
|
||||
|
||||
# Check / set dependent variables for MATLAB wrapper
|
||||
if(GTSAM_INSTALL_MATLAB_TOOLBOX AND GTSAM_BUILD_TYPE_POSTFIXES)
|
||||
set(CURRENT_POSTFIX ${CMAKE_${CMAKE_BUILD_TYPE_UPPER}_POSTFIX})
|
||||
endif()
|
||||
|
||||
if(GTSAM_INSTALL_MATLAB_TOOLBOX AND NOT BUILD_SHARED_LIBS)
|
||||
message(FATAL_ERROR "GTSAM_INSTALL_MATLAB_TOOLBOX and BUILD_SHARED_LIBS=OFF. The MATLAB wrapper cannot be compiled with a static GTSAM library because mex modules are themselves shared libraries. If you want a self-contained mex module, enable GTSAM_MEX_BUILD_STATIC_MODULE instead of BUILD_SHARED_LIBS=OFF.")
|
||||
endif()
|
||||
|
|
|
|||
|
|
@ -125,7 +125,7 @@ int main(int argc, char* argv[]) {
|
|||
output_filename = var_map["output_filename"].as<string>();
|
||||
use_isam = var_map["use_isam"].as<bool>();
|
||||
|
||||
ISAM2* isam2;
|
||||
ISAM2* isam2 = 0;
|
||||
if (use_isam) {
|
||||
printf("Using ISAM2\n");
|
||||
ISAM2Params parameters;
|
||||
|
|
|
|||
|
|
@ -199,7 +199,7 @@ if(WIN32)
|
|||
else()
|
||||
if("${CMAKE_BUILD_TYPE}" STREQUAL "Release")
|
||||
# Suppress all warnings from 3rd party sources.
|
||||
set_source_files_properties(${3rdparty_srcs} PROPERTIES COMPILE_FLAGS "-w")
|
||||
set_source_files_properties(${3rdparty_srcs} PROPERTIES COMPILE_FLAGS "-w -Wno-everything")
|
||||
else()
|
||||
set_source_files_properties(${3rdparty_srcs} PROPERTIES COMPILE_FLAGS "-Wno-error")
|
||||
endif()
|
||||
|
|
|
|||
|
|
@ -0,0 +1,66 @@
|
|||
/* ----------------------------------------------------------------------------
|
||||
|
||||
* 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 Cal3.h
|
||||
* @brief Common code for all Calibration models.
|
||||
* @author Varun Agrawal
|
||||
*/
|
||||
|
||||
/**
|
||||
* @addtogroup geometry
|
||||
*/
|
||||
|
||||
#pragma once
|
||||
|
||||
#include <gtsam/geometry/Point2.h>
|
||||
|
||||
namespace gtsam {
|
||||
|
||||
/**
|
||||
* Function which makes use of the Implicit Function Theorem to compute the
|
||||
* Jacobians of `calibrate` using `uncalibrate`.
|
||||
* This is useful when there are iterative operations in the `calibrate`
|
||||
* function which make computing jacobians difficult.
|
||||
*
|
||||
* Given f(pi, pn) = uncalibrate(pn) - pi, and g(pi) = calibrate, we can
|
||||
* easily compute the Jacobians:
|
||||
* df/pi = -I (pn and pi are independent args)
|
||||
* Dp = -inv(H_uncal_pn) * df/pi = -inv(H_uncal_pn) * (-I) = inv(H_uncal_pn)
|
||||
* Dcal = -inv(H_uncal_pn) * df/K = -inv(H_uncal_pn) * H_uncal_K
|
||||
*
|
||||
* @tparam Cal Calibration model.
|
||||
* @tparam Dim The number of parameters in the calibration model.
|
||||
* @param p Calibrated point.
|
||||
* @param Dcal optional 2*p Jacobian wrpt `p` Cal3DS2 parameters.
|
||||
* @param Dp optional 2*2 Jacobian wrpt intrinsic coordinates.
|
||||
*/
|
||||
template <typename Cal, size_t Dim>
|
||||
void calibrateJacobians(const Cal& calibration, const Point2& pn,
|
||||
OptionalJacobian<2, Dim> Dcal = boost::none,
|
||||
OptionalJacobian<2, 2> Dp = boost::none) {
|
||||
if (Dcal || Dp) {
|
||||
Eigen::Matrix<double, 2, Dim> H_uncal_K;
|
||||
Matrix22 H_uncal_pn, H_uncal_pn_inv;
|
||||
|
||||
// Compute uncalibrate Jacobians
|
||||
calibration.uncalibrate(pn, Dcal ? &H_uncal_K : nullptr, H_uncal_pn);
|
||||
|
||||
H_uncal_pn_inv = H_uncal_pn.inverse();
|
||||
|
||||
if (Dp) *Dp = H_uncal_pn_inv;
|
||||
if (Dcal) *Dcal = -H_uncal_pn_inv * H_uncal_K;
|
||||
}
|
||||
}
|
||||
|
||||
//TODO(Varun) Make common base class for all calibration models.
|
||||
|
||||
} // \ namespace gtsam
|
||||
|
|
@ -121,24 +121,7 @@ Point2 Cal3Bundler::calibrate(const Point2& pi,
|
|||
throw std::runtime_error(
|
||||
"Cal3Bundler::calibrate fails to converge. need a better initialization");
|
||||
|
||||
// We make use of the Implicit Function Theorem to compute the Jacobians from uncalibrate
|
||||
// Given f(pi, pn) = uncalibrate(pn) - pi, and g(pi) = calibrate, we can easily compute the Jacobians
|
||||
// df/pi = -I (pn and pi are independent args)
|
||||
// Dcal = -inv(H_uncal_pn) * df/pi = -inv(H_uncal_pn) * (-I) = inv(H_uncal_pn)
|
||||
// Dp = -inv(H_uncal_pn) * df/K = -inv(H_uncal_pn) * H_uncal_K
|
||||
Matrix23 H_uncal_K;
|
||||
Matrix22 H_uncal_pn, H_uncal_pn_inv;
|
||||
|
||||
if (Dcal || Dp) {
|
||||
// Compute uncalibrate Jacobians
|
||||
uncalibrate(pn, Dcal ? &H_uncal_K : nullptr, H_uncal_pn);
|
||||
|
||||
H_uncal_pn_inv = H_uncal_pn.inverse();
|
||||
|
||||
if (Dp) *Dp = H_uncal_pn_inv;
|
||||
if (Dcal) *Dcal = -H_uncal_pn_inv * H_uncal_K;
|
||||
|
||||
}
|
||||
calibrateJacobians<Cal3Bundler, dimension>(*this, pn, Dcal, Dp);
|
||||
|
||||
return pn;
|
||||
}
|
||||
|
|
|
|||
|
|
@ -18,6 +18,7 @@
|
|||
|
||||
#pragma once
|
||||
|
||||
#include <gtsam/geometry/Cal3.h>
|
||||
#include <gtsam/geometry/Point2.h>
|
||||
|
||||
namespace gtsam {
|
||||
|
|
|
|||
|
|
@ -44,8 +44,8 @@ public:
|
|||
Cal3DS2() : Base() {}
|
||||
|
||||
Cal3DS2(double fx, double fy, double s, double u0, double v0,
|
||||
double k1, double k2, double p1 = 0.0, double p2 = 0.0) :
|
||||
Base(fx, fy, s, u0, v0, k1, k2, p1, p2) {}
|
||||
double k1, double k2, double p1 = 0.0, double p2 = 0.0, double tol = 1e-5) :
|
||||
Base(fx, fy, s, u0, v0, k1, k2, p1, p2, tol) {}
|
||||
|
||||
virtual ~Cal3DS2() {}
|
||||
|
||||
|
|
|
|||
|
|
@ -13,6 +13,7 @@
|
|||
* @file Cal3DS2_Base.cpp
|
||||
* @date Feb 28, 2010
|
||||
* @author ydjian
|
||||
* @author Varun Agrawal
|
||||
*/
|
||||
|
||||
#include <gtsam/base/Vector.h>
|
||||
|
|
@ -24,8 +25,16 @@
|
|||
namespace gtsam {
|
||||
|
||||
/* ************************************************************************* */
|
||||
Cal3DS2_Base::Cal3DS2_Base(const Vector &v):
|
||||
fx_(v[0]), fy_(v[1]), s_(v[2]), u0_(v[3]), v0_(v[4]), k1_(v[5]), k2_(v[6]), p1_(v[7]), p2_(v[8]){}
|
||||
Cal3DS2_Base::Cal3DS2_Base(const Vector& v)
|
||||
: fx_(v(0)),
|
||||
fy_(v(1)),
|
||||
s_(v(2)),
|
||||
u0_(v(3)),
|
||||
v0_(v(4)),
|
||||
k1_(v(5)),
|
||||
k2_(v(6)),
|
||||
p1_(v(7)),
|
||||
p2_(v(8)) {}
|
||||
|
||||
/* ************************************************************************* */
|
||||
Matrix3 Cal3DS2_Base::K() const {
|
||||
|
|
@ -94,9 +103,8 @@ static Matrix2 D2dintrinsic(double x, double y, double rr,
|
|||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
Point2 Cal3DS2_Base::uncalibrate(const Point2& p,
|
||||
OptionalJacobian<2,9> H1, OptionalJacobian<2,2> H2) const {
|
||||
|
||||
Point2 Cal3DS2_Base::uncalibrate(const Point2& p, OptionalJacobian<2, 9> Dcal,
|
||||
OptionalJacobian<2, 2> Dp) const {
|
||||
// rr = x^2 + y^2;
|
||||
// g = (1 + k(1)*rr + k(2)*rr^2);
|
||||
// dp = [2*k(3)*x*y + k(4)*(rr + 2*x^2); 2*k(4)*x*y + k(3)*(rr + 2*y^2)];
|
||||
|
|
@ -115,37 +123,44 @@ Point2 Cal3DS2_Base::uncalibrate(const Point2& p,
|
|||
const double pny = g * y + dy;
|
||||
|
||||
Matrix2 DK;
|
||||
if (H1 || H2) DK << fx_, s_, 0.0, fy_;
|
||||
if (Dcal || Dp) {
|
||||
DK << fx_, s_, 0.0, fy_;
|
||||
}
|
||||
|
||||
// Derivative for calibration
|
||||
if (H1)
|
||||
*H1 = D2dcalibration(x, y, xx, yy, xy, rr, r4, pnx, pny, DK);
|
||||
if (Dcal) {
|
||||
*Dcal = D2dcalibration(x, y, xx, yy, xy, rr, r4, pnx, pny, DK);
|
||||
}
|
||||
|
||||
// Derivative for points
|
||||
if (H2)
|
||||
*H2 = D2dintrinsic(x, y, rr, g, k1_, k2_, p1_, p2_, DK);
|
||||
if (Dp) {
|
||||
*Dp = D2dintrinsic(x, y, rr, g, k1_, k2_, p1_, p2_, DK);
|
||||
}
|
||||
|
||||
// Regular uncalibrate after distortion
|
||||
return Point2(fx_ * pnx + s_ * pny + u0_, fy_ * pny + v0_);
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
Point2 Cal3DS2_Base::calibrate(const Point2& pi, const double tol) const {
|
||||
Point2 Cal3DS2_Base::calibrate(const Point2& pi, OptionalJacobian<2, 9> Dcal,
|
||||
OptionalJacobian<2, 2> Dp) const {
|
||||
// Use the following fixed point iteration to invert the radial distortion.
|
||||
// pn_{t+1} = (inv(K)*pi - dp(pn_{t})) / g(pn_{t})
|
||||
|
||||
const Point2 invKPi ((1 / fx_) * (pi.x() - u0_ - (s_ / fy_) * (pi.y() - v0_)),
|
||||
(1 / fy_) * (pi.y() - v0_));
|
||||
const Point2 invKPi((1 / fx_) * (pi.x() - u0_ - (s_ / fy_) * (pi.y() - v0_)),
|
||||
(1 / fy_) * (pi.y() - v0_));
|
||||
|
||||
// initialize by ignoring the distortion at all, might be problematic for pixels around boundary
|
||||
// initialize by ignoring the distortion at all, might be problematic for
|
||||
// pixels around boundary
|
||||
Point2 pn = invKPi;
|
||||
|
||||
// iterate until the uncalibrate is close to the actual pixel coordinate
|
||||
const int maxIterations = 10;
|
||||
int iteration;
|
||||
for (iteration = 0; iteration < maxIterations; ++iteration) {
|
||||
if (distance2(uncalibrate(pn), pi) <= tol) break;
|
||||
const double x = pn.x(), y = pn.y(), xy = x * y, xx = x * x, yy = y * y;
|
||||
if (distance2(uncalibrate(pn), pi) <= tol_) break;
|
||||
const double px = pn.x(), py = pn.y(), xy = px * py, xx = px * px,
|
||||
yy = py * py;
|
||||
const double rr = xx + yy;
|
||||
const double g = (1 + k1_ * rr + k2_ * rr * rr);
|
||||
const double dx = 2 * p1_ * xy + p2_ * (rr + 2 * xx);
|
||||
|
|
@ -153,8 +168,11 @@ Point2 Cal3DS2_Base::calibrate(const Point2& pi, const double tol) const {
|
|||
pn = (invKPi - Point2(dx, dy)) / g;
|
||||
}
|
||||
|
||||
if ( iteration >= maxIterations )
|
||||
throw std::runtime_error("Cal3DS2::calibrate fails to converge. need a better initialization");
|
||||
if (iteration >= maxIterations)
|
||||
throw std::runtime_error(
|
||||
"Cal3DS2::calibrate fails to converge. need a better initialization");
|
||||
|
||||
calibrateJacobians<Cal3DS2_Base, dimension>(*this, pn, Dcal, Dp);
|
||||
|
||||
return pn;
|
||||
}
|
||||
|
|
|
|||
|
|
@ -14,10 +14,12 @@
|
|||
* @brief Calibration of a camera with radial distortion
|
||||
* @date Feb 28, 2010
|
||||
* @author ydjian
|
||||
* @author Varun Agrawal
|
||||
*/
|
||||
|
||||
#pragma once
|
||||
|
||||
#include <gtsam/geometry/Cal3.h>
|
||||
#include <gtsam/geometry/Point2.h>
|
||||
|
||||
namespace gtsam {
|
||||
|
|
@ -39,22 +41,43 @@ namespace gtsam {
|
|||
class GTSAM_EXPORT Cal3DS2_Base {
|
||||
|
||||
protected:
|
||||
|
||||
double fx_, fy_, s_, u0_, v0_ ; // focal length, skew and principal point
|
||||
double k1_, k2_ ; // radial 2nd-order and 4th-order
|
||||
double p1_, p2_ ; // tangential distortion
|
||||
double fx_, fy_, s_, u0_, v0_; // focal length, skew and principal point
|
||||
double k1_, k2_; // radial 2nd-order and 4th-order
|
||||
double p1_, p2_; // tangential distortion
|
||||
double tol_ = 1e-5; // tolerance value when calibrating
|
||||
|
||||
public:
|
||||
|
||||
enum { dimension = 9 };
|
||||
|
||||
/// @name Standard Constructors
|
||||
/// @{
|
||||
|
||||
/// Default Constructor with only unit focal length
|
||||
Cal3DS2_Base() : fx_(1), fy_(1), s_(0), u0_(0), v0_(0), k1_(0), k2_(0), p1_(0), p2_(0) {}
|
||||
/// Default Constructor with only unit focal length
|
||||
Cal3DS2_Base()
|
||||
: fx_(1),
|
||||
fy_(1),
|
||||
s_(0),
|
||||
u0_(0),
|
||||
v0_(0),
|
||||
k1_(0),
|
||||
k2_(0),
|
||||
p1_(0),
|
||||
p2_(0),
|
||||
tol_(1e-5) {}
|
||||
|
||||
Cal3DS2_Base(double fx, double fy, double s, double u0, double v0,
|
||||
double k1, double k2, double p1 = 0.0, double p2 = 0.0) :
|
||||
fx_(fx), fy_(fy), s_(s), u0_(u0), v0_(v0), k1_(k1), k2_(k2), p1_(p1), p2_(p2) {}
|
||||
Cal3DS2_Base(double fx, double fy, double s, double u0, double v0, double k1,
|
||||
double k2, double p1 = 0.0, double p2 = 0.0, double tol = 1e-5)
|
||||
: fx_(fx),
|
||||
fy_(fy),
|
||||
s_(s),
|
||||
u0_(u0),
|
||||
v0_(v0),
|
||||
k1_(k1),
|
||||
k2_(k2),
|
||||
p1_(p1),
|
||||
p2_(p2),
|
||||
tol_(tol) {}
|
||||
|
||||
virtual ~Cal3DS2_Base() {}
|
||||
|
||||
|
|
@ -72,7 +95,7 @@ public:
|
|||
virtual void print(const std::string& s = "") const;
|
||||
|
||||
/// assert equality up to a tolerance
|
||||
bool equals(const Cal3DS2_Base& K, double tol = 10e-9) const;
|
||||
bool equals(const Cal3DS2_Base& K, double tol = 1e-8) const;
|
||||
|
||||
/// @}
|
||||
/// @name Standard Interface
|
||||
|
|
@ -121,12 +144,12 @@ public:
|
|||
* @param Dp optional 2*2 Jacobian wrpt intrinsic coordinates
|
||||
* @return point in (distorted) image coordinates
|
||||
*/
|
||||
Point2 uncalibrate(const Point2& p,
|
||||
OptionalJacobian<2,9> Dcal = boost::none,
|
||||
OptionalJacobian<2,2> Dp = boost::none) const ;
|
||||
Point2 uncalibrate(const Point2& p, OptionalJacobian<2, 9> Dcal = boost::none,
|
||||
OptionalJacobian<2, 2> Dp = boost::none) const;
|
||||
|
||||
/// Convert (distorted) image coordinates uv to intrinsic coordinates xy
|
||||
Point2 calibrate(const Point2& p, const double tol=1e-5) const;
|
||||
Point2 calibrate(const Point2& p, OptionalJacobian<2, 9> Dcal = boost::none,
|
||||
OptionalJacobian<2, 2> Dp = boost::none) const;
|
||||
|
||||
/// Derivative of uncalibrate wrpt intrinsic coordinates
|
||||
Matrix2 D2d_intrinsic(const Point2& p) const ;
|
||||
|
|
@ -164,6 +187,7 @@ private:
|
|||
ar & BOOST_SERIALIZATION_NVP(k2_);
|
||||
ar & BOOST_SERIALIZATION_NVP(p1_);
|
||||
ar & BOOST_SERIALIZATION_NVP(p2_);
|
||||
ar & BOOST_SERIALIZATION_NVP(tol_);
|
||||
}
|
||||
|
||||
/// @}
|
||||
|
|
|
|||
|
|
@ -122,14 +122,15 @@ Point2 Cal3Fisheye::uncalibrate(const Point2& p, OptionalJacobian<2, 9> H1,
|
|||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
Point2 Cal3Fisheye::calibrate(const Point2& uv, const double tol) const {
|
||||
Point2 Cal3Fisheye::calibrate(const Point2& uv, OptionalJacobian<2, 9> Dcal,
|
||||
OptionalJacobian<2, 2> Dp) const {
|
||||
// initial gues just inverts the pinhole model
|
||||
const double u = uv.x(), v = uv.y();
|
||||
const double yd = (v - v0_) / fy_;
|
||||
const double xd = (u - s_ * yd - u0_) / fx_;
|
||||
Point2 pi(xd, yd);
|
||||
|
||||
// Perform newtons method, break when solution converges past tol,
|
||||
// Perform newtons method, break when solution converges past tol_,
|
||||
// throw exception if max iterations are reached
|
||||
const int maxIterations = 10;
|
||||
int iteration;
|
||||
|
|
@ -140,7 +141,7 @@ Point2 Cal3Fisheye::calibrate(const Point2& uv, const double tol) const {
|
|||
const Point2 uv_hat = uncalibrate(pi, boost::none, jac);
|
||||
|
||||
// Test convergence
|
||||
if ((uv_hat - uv).norm() < tol) break;
|
||||
if ((uv_hat - uv).norm() < tol_) break;
|
||||
|
||||
// Newton's method update step
|
||||
pi = pi - jac.inverse() * (uv_hat - uv);
|
||||
|
|
@ -151,6 +152,8 @@ Point2 Cal3Fisheye::calibrate(const Point2& uv, const double tol) const {
|
|||
"Cal3Fisheye::calibrate fails to converge. need a better "
|
||||
"initialization");
|
||||
|
||||
calibrateJacobians<Cal3Fisheye, dimension>(*this, pi, Dcal, Dp);
|
||||
|
||||
return pi;
|
||||
}
|
||||
|
||||
|
|
|
|||
|
|
@ -14,10 +14,12 @@
|
|||
* @brief Calibration of a fisheye camera
|
||||
* @date Apr 8, 2020
|
||||
* @author ghaggin
|
||||
* @author Varun Agrawal
|
||||
*/
|
||||
|
||||
#pragma once
|
||||
|
||||
#include <gtsam/geometry/Cal3.h>
|
||||
#include <gtsam/geometry/Point2.h>
|
||||
|
||||
#include <string>
|
||||
|
|
@ -48,6 +50,7 @@ class GTSAM_EXPORT Cal3Fisheye {
|
|||
private:
|
||||
double fx_, fy_, s_, u0_, v0_; // focal length, skew and principal point
|
||||
double k1_, k2_, k3_, k4_; // fisheye distortion coefficients
|
||||
double tol_ = 1e-5; // tolerance value when calibrating
|
||||
|
||||
public:
|
||||
enum { dimension = 9 };
|
||||
|
|
@ -59,11 +62,11 @@ class GTSAM_EXPORT Cal3Fisheye {
|
|||
|
||||
/// Default Constructor with only unit focal length
|
||||
Cal3Fisheye()
|
||||
: fx_(1), fy_(1), s_(0), u0_(0), v0_(0), k1_(0), k2_(0), k3_(0), k4_(0) {}
|
||||
: fx_(1), fy_(1), s_(0), u0_(0), v0_(0), k1_(0), k2_(0), k3_(0), k4_(0), tol_(1e-5) {}
|
||||
|
||||
Cal3Fisheye(const double fx, const double fy, const double s, const double u0,
|
||||
const double v0, const double k1, const double k2,
|
||||
const double k3, const double k4)
|
||||
const double k3, const double k4, double tol = 1e-5)
|
||||
: fx_(fx),
|
||||
fy_(fy),
|
||||
s_(s),
|
||||
|
|
@ -72,7 +75,8 @@ class GTSAM_EXPORT Cal3Fisheye {
|
|||
k1_(k1),
|
||||
k2_(k2),
|
||||
k3_(k3),
|
||||
k4_(k4) {}
|
||||
k4_(k4),
|
||||
tol_(tol) {}
|
||||
|
||||
virtual ~Cal3Fisheye() {}
|
||||
|
||||
|
|
@ -139,7 +143,8 @@ class GTSAM_EXPORT Cal3Fisheye {
|
|||
|
||||
/// Convert (distorted) image coordinates [u;v] to intrinsic coordinates [x_i,
|
||||
/// y_i]
|
||||
Point2 calibrate(const Point2& p, const double tol = 1e-5) const;
|
||||
Point2 calibrate(const Point2& p, OptionalJacobian<2, 9> Dcal = boost::none,
|
||||
OptionalJacobian<2, 2> Dp = boost::none) const;
|
||||
|
||||
/// @}
|
||||
/// @name Testable
|
||||
|
|
|
|||
|
|
@ -13,6 +13,7 @@
|
|||
* @file Cal3Unified.cpp
|
||||
* @date Mar 8, 2014
|
||||
* @author Jing Dong
|
||||
* @author Varun Agrawal
|
||||
*/
|
||||
|
||||
#include <gtsam/base/Vector.h>
|
||||
|
|
@ -54,8 +55,8 @@ bool Cal3Unified::equals(const Cal3Unified& K, double tol) const {
|
|||
/* ************************************************************************* */
|
||||
// todo: make a fixed sized jacobian version of this
|
||||
Point2 Cal3Unified::uncalibrate(const Point2& p,
|
||||
OptionalJacobian<2,10> H1,
|
||||
OptionalJacobian<2,2> H2) const {
|
||||
OptionalJacobian<2,10> Dcal,
|
||||
OptionalJacobian<2,2> Dp) const {
|
||||
|
||||
// this part of code is modified from Cal3DS2,
|
||||
// since the second part of this model (after project to normalized plane)
|
||||
|
|
@ -78,16 +79,16 @@ Point2 Cal3Unified::uncalibrate(const Point2& p,
|
|||
Point2 puncalib = Base::uncalibrate(m, H1base, H2base);
|
||||
|
||||
// Inlined derivative for calibration
|
||||
if (H1) {
|
||||
if (Dcal) {
|
||||
// part1
|
||||
Vector2 DU;
|
||||
DU << -xs * sqrt_nx * xi_sqrt_nx2, //
|
||||
-ys * sqrt_nx * xi_sqrt_nx2;
|
||||
*H1 << H1base, H2base * DU;
|
||||
*Dcal << H1base, H2base * DU;
|
||||
}
|
||||
|
||||
// Inlined derivative for points
|
||||
if (H2) {
|
||||
if (Dp) {
|
||||
// part1
|
||||
const double denom = 1.0 * xi_sqrt_nx2 / sqrt_nx;
|
||||
const double mid = -(xi * xs*ys) * denom;
|
||||
|
|
@ -95,20 +96,24 @@ Point2 Cal3Unified::uncalibrate(const Point2& p,
|
|||
DU << (sqrt_nx + xi*(ys*ys + 1)) * denom, mid, //
|
||||
mid, (sqrt_nx + xi*(xs*xs + 1)) * denom;
|
||||
|
||||
*H2 << H2base * DU;
|
||||
*Dp << H2base * DU;
|
||||
}
|
||||
|
||||
return puncalib;
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
Point2 Cal3Unified::calibrate(const Point2& pi, const double tol) const {
|
||||
|
||||
Point2 Cal3Unified::calibrate(const Point2& pi, OptionalJacobian<2, 10> Dcal,
|
||||
OptionalJacobian<2, 2> Dp) const {
|
||||
// calibrate point to Nplane use base class::calibrate()
|
||||
Point2 pnplane = Base::calibrate(pi, tol);
|
||||
Point2 pnplane = Base::calibrate(pi);
|
||||
|
||||
// call nplane to space
|
||||
return this->nPlaneToSpace(pnplane);
|
||||
Point2 pn = this->nPlaneToSpace(pnplane);
|
||||
|
||||
calibrateJacobians<Cal3Unified, dimension>(*this, pn, Dcal, Dp);
|
||||
|
||||
return pn;
|
||||
}
|
||||
/* ************************************************************************* */
|
||||
Point2 Cal3Unified::nPlaneToSpace(const Point2& p) const {
|
||||
|
|
|
|||
|
|
@ -14,6 +14,7 @@
|
|||
* @brief Unified Calibration Model, see Mei07icra for details
|
||||
* @date Mar 8, 2014
|
||||
* @author Jing Dong
|
||||
* @author Varun Agrawal
|
||||
*/
|
||||
|
||||
/**
|
||||
|
|
@ -99,7 +100,8 @@ public:
|
|||
OptionalJacobian<2,2> Dp = boost::none) const ;
|
||||
|
||||
/// Conver a pixel coordinate to ideal coordinate
|
||||
Point2 calibrate(const Point2& p, const double tol=1e-5) const;
|
||||
Point2 calibrate(const Point2& p, OptionalJacobian<2, 10> Dcal = boost::none,
|
||||
OptionalJacobian<2, 2> Dp = boost::none) const;
|
||||
|
||||
/// Convert a 3D point to normalized unit plane
|
||||
Point2 spaceToNPlane(const Point2& p) const;
|
||||
|
|
|
|||
|
|
@ -21,6 +21,7 @@
|
|||
|
||||
namespace gtsam {
|
||||
|
||||
#ifdef GTSAM_ALLOW_DEPRECATED_SINCE_V41
|
||||
SimpleCamera simpleCamera(const Matrix34& P) {
|
||||
|
||||
// P = [A|a] = s K cRw [I|-T], with s the unknown scale
|
||||
|
|
@ -45,5 +46,6 @@ namespace gtsam {
|
|||
return SimpleCamera(Pose3(wRc, T),
|
||||
Cal3_S2(K(0, 0), K(1, 1), K(0, 1), K(0, 2), K(1, 2)));
|
||||
}
|
||||
#endif
|
||||
|
||||
}
|
||||
|
|
|
|||
|
|
@ -19,14 +19,22 @@
|
|||
#pragma once
|
||||
|
||||
#include <gtsam/geometry/BearingRange.h>
|
||||
#include <gtsam/geometry/PinholeCamera.h>
|
||||
#include <gtsam/geometry/Cal3Bundler.h>
|
||||
#include <gtsam/geometry/Cal3DS2.h>
|
||||
#include <gtsam/geometry/Cal3Unified.h>
|
||||
#include <gtsam/geometry/Cal3_S2.h>
|
||||
#include <gtsam/geometry/PinholeCamera.h>
|
||||
|
||||
namespace gtsam {
|
||||
|
||||
/// A simple camera class with a Cal3_S2 calibration
|
||||
typedef gtsam::PinholeCamera<gtsam::Cal3_S2> PinholeCameraCal3_S2;
|
||||
/// Convenient aliases for Pinhole camera classes with different calibrations.
|
||||
/// Also needed as forward declarations in the wrapper.
|
||||
using PinholeCameraCal3_S2 = gtsam::PinholeCamera<gtsam::Cal3_S2>;
|
||||
using PinholeCameraCal3Bundler = gtsam::PinholeCamera<gtsam::Cal3Bundler>;
|
||||
using PinholeCameraCal3DS2 = gtsam::PinholeCamera<gtsam::Cal3DS2>;
|
||||
using PinholeCameraCal3Unified = gtsam::PinholeCamera<gtsam::Cal3Unified>;
|
||||
|
||||
#ifdef GTSAM_ALLOW_DEPRECATED_SINCE_V41
|
||||
/**
|
||||
* @deprecated: SimpleCamera for backwards compatability with GTSAM 3.x
|
||||
* Use PinholeCameraCal3_S2 instead
|
||||
|
|
@ -140,4 +148,6 @@ struct traits<const SimpleCamera> : public internal::Manifold<SimpleCamera> {};
|
|||
template <typename T>
|
||||
struct Range<SimpleCamera, T> : HasRange<SimpleCamera, T, double> {};
|
||||
|
||||
#endif
|
||||
|
||||
} // namespace gtsam
|
||||
|
|
|
|||
|
|
@ -181,6 +181,23 @@ TEST(Cal3Fisheye, calibrate3) {
|
|||
CHECK(assert_equal(xi_hat, xi));
|
||||
}
|
||||
|
||||
Point2 calibrate_(const Cal3Fisheye& k, const Point2& pt) {
|
||||
return k.calibrate(pt);
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
TEST(Cal3Fisheye, Dcalibrate)
|
||||
{
|
||||
Point2 p(0.5, 0.5);
|
||||
Point2 pi = K.uncalibrate(p);
|
||||
Matrix Dcal, Dp;
|
||||
K.calibrate(pi, Dcal, Dp);
|
||||
Matrix numerical1 = numericalDerivative21(calibrate_, K, pi);
|
||||
CHECK(assert_equal(numerical1,Dcal,1e-5));
|
||||
Matrix numerical2 = numericalDerivative22(calibrate_, K, pi);
|
||||
CHECK(assert_equal(numerical2,Dp,1e-5));
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
int main() {
|
||||
TestResult tr;
|
||||
|
|
|
|||
|
|
@ -74,12 +74,26 @@ TEST( Cal3DS2, Duncalibrate2)
|
|||
CHECK(assert_equal(numerical,separate,1e-5));
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
TEST( Cal3DS2, assert_equal)
|
||||
{
|
||||
CHECK(assert_equal(K,K,1e-5));
|
||||
Point2 calibrate_(const Cal3DS2& k, const Point2& pt) {
|
||||
return k.calibrate(pt);
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
TEST( Cal3DS2, Dcalibrate)
|
||||
{
|
||||
Point2 pn(0.5, 0.5);
|
||||
Point2 pi = K.uncalibrate(pn);
|
||||
Matrix Dcal, Dp;
|
||||
K.calibrate(pi, Dcal, Dp);
|
||||
Matrix numerical1 = numericalDerivative21(calibrate_, K, pi, 1e-7);
|
||||
CHECK(assert_equal(numerical1, Dcal, 1e-5));
|
||||
Matrix numerical2 = numericalDerivative22(calibrate_, K, pi, 1e-7);
|
||||
CHECK(assert_equal(numerical2, Dp, 1e-5));
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
TEST(Cal3DS2, assert_equal) { CHECK(assert_equal(K, K, 1e-5)); }
|
||||
|
||||
/* ************************************************************************* */
|
||||
TEST( Cal3DS2, retract)
|
||||
{
|
||||
|
|
|
|||
|
|
@ -82,6 +82,22 @@ TEST( Cal3Unified, Duncalibrate2)
|
|||
CHECK(assert_equal(numerical,computed,1e-6));
|
||||
}
|
||||
|
||||
Point2 calibrate_(const Cal3Unified& k, const Point2& pt) {
|
||||
return k.calibrate(pt);
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
TEST( Cal3Unified, Dcalibrate)
|
||||
{
|
||||
Point2 pi = K.uncalibrate(p);
|
||||
Matrix Dcal, Dp;
|
||||
K.calibrate(pi, Dcal, Dp);
|
||||
Matrix numerical1 = numericalDerivative21(calibrate_, K, pi);
|
||||
CHECK(assert_equal(numerical1,Dcal,1e-5));
|
||||
Matrix numerical2 = numericalDerivative22(calibrate_, K, pi);
|
||||
CHECK(assert_equal(numerical2,Dp,1e-5));
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
TEST( Cal3Unified, assert_equal)
|
||||
{
|
||||
|
|
|
|||
|
|
@ -26,6 +26,8 @@
|
|||
using namespace std;
|
||||
using namespace gtsam;
|
||||
|
||||
#ifdef GTSAM_ALLOW_DEPRECATED_SINCE_V41
|
||||
|
||||
static const Cal3_S2 K(625, 625, 0, 0, 0);
|
||||
|
||||
static const Pose3 pose1(Rot3(Vector3(1, -1, -1).asDiagonal()),
|
||||
|
|
@ -149,6 +151,8 @@ TEST( SimpleCamera, simpleCamera)
|
|||
CHECK(assert_equal(expected, actual,1e-1));
|
||||
}
|
||||
|
||||
#endif
|
||||
|
||||
/* ************************************************************************* */
|
||||
int main() { TestResult tr; return TestRegistry::runAllTests(tr); }
|
||||
/* ************************************************************************* */
|
||||
|
|
|
|||
|
|
@ -329,7 +329,7 @@ virtual class Value {
|
|||
};
|
||||
|
||||
#include <gtsam/base/GenericValue.h>
|
||||
template<T = {Vector, Matrix, gtsam::Point2, gtsam::Point3, gtsam::Rot2, gtsam::Rot3, gtsam::Pose2, gtsam::Pose3, gtsam::StereoPoint2, gtsam::Cal3_S2, gtsam::Cal3DS2, gtsam::Cal3Bundler, gtsam::EssentialMatrix, gtsam::CalibratedCamera, gtsam::SimpleCamera, gtsam::imuBias::ConstantBias}>
|
||||
template<T = {Vector, Matrix, gtsam::Point2, gtsam::Point3, gtsam::Rot2, gtsam::Rot3, gtsam::Pose2, gtsam::Pose3, gtsam::StereoPoint2, gtsam::Cal3_S2, gtsam::Cal3DS2, gtsam::Cal3Bundler, gtsam::EssentialMatrix, gtsam::CalibratedCamera, gtsam::imuBias::ConstantBias}>
|
||||
virtual class GenericValue : gtsam::Value {
|
||||
void serializable() const;
|
||||
};
|
||||
|
|
@ -881,7 +881,7 @@ virtual class Cal3DS2_Base {
|
|||
|
||||
// Action on Point2
|
||||
gtsam::Point2 uncalibrate(const gtsam::Point2& p) const;
|
||||
gtsam::Point2 calibrate(const gtsam::Point2& p, double tol) const;
|
||||
gtsam::Point2 calibrate(const gtsam::Point2& p) const;
|
||||
|
||||
// enabling serialization functionality
|
||||
void serialize() const;
|
||||
|
|
@ -1059,54 +1059,13 @@ class PinholeCamera {
|
|||
void serialize() const;
|
||||
};
|
||||
|
||||
// Forward declaration of PinholeCameraCalX is defined here.
|
||||
#include <gtsam/geometry/SimpleCamera.h>
|
||||
virtual class SimpleCamera {
|
||||
// Standard Constructors and Named Constructors
|
||||
SimpleCamera();
|
||||
SimpleCamera(const gtsam::Pose3& pose);
|
||||
SimpleCamera(const gtsam::Pose3& pose, const gtsam::Cal3_S2& K);
|
||||
static gtsam::SimpleCamera Level(const gtsam::Cal3_S2& K, const gtsam::Pose2& pose, double height);
|
||||
static gtsam::SimpleCamera Level(const gtsam::Pose2& pose, double height);
|
||||
static gtsam::SimpleCamera Lookat(const gtsam::Point3& eye, const gtsam::Point3& target,
|
||||
const gtsam::Point3& upVector, const gtsam::Cal3_S2& K);
|
||||
static gtsam::SimpleCamera Lookat(const gtsam::Point3& eye, const gtsam::Point3& target,
|
||||
const gtsam::Point3& upVector);
|
||||
|
||||
// Testable
|
||||
void print(string s) const;
|
||||
bool equals(const gtsam::SimpleCamera& camera, double tol) const;
|
||||
|
||||
// Standard Interface
|
||||
gtsam::Pose3 pose() const;
|
||||
gtsam::Cal3_S2 calibration() const;
|
||||
|
||||
// Manifold
|
||||
gtsam::SimpleCamera retract(Vector d) const;
|
||||
Vector localCoordinates(const gtsam::SimpleCamera& T2) const;
|
||||
size_t dim() const;
|
||||
static size_t Dim();
|
||||
|
||||
// Transformations and measurement functions
|
||||
static gtsam::Point2 Project(const gtsam::Point3& cameraPoint);
|
||||
pair<gtsam::Point2,bool> projectSafe(const gtsam::Point3& pw) const;
|
||||
gtsam::Point2 project(const gtsam::Point3& point);
|
||||
gtsam::Point3 backproject(const gtsam::Point2& p, double depth) const;
|
||||
double range(const gtsam::Point3& point);
|
||||
double range(const gtsam::Pose3& pose);
|
||||
|
||||
// enabling serialization functionality
|
||||
void serialize() const;
|
||||
|
||||
};
|
||||
|
||||
gtsam::SimpleCamera simpleCamera(const Matrix& P);
|
||||
|
||||
// Some typedefs for common camera types
|
||||
// PinholeCameraCal3_S2 is the same as SimpleCamera above
|
||||
typedef gtsam::PinholeCamera<gtsam::Cal3_S2> PinholeCameraCal3_S2;
|
||||
//TODO (Issue 237) due to lack of jacobians of Cal3DS2_Base::calibrate, PinholeCamera does not apply to Cal3DS2/Unified
|
||||
//typedef gtsam::PinholeCamera<gtsam::Cal3DS2> PinholeCameraCal3DS2;
|
||||
//typedef gtsam::PinholeCamera<gtsam::Cal3Unified> PinholeCameraCal3Unified;
|
||||
typedef gtsam::PinholeCamera<gtsam::Cal3DS2> PinholeCameraCal3DS2;
|
||||
typedef gtsam::PinholeCamera<gtsam::Cal3Unified> PinholeCameraCal3Unified;
|
||||
typedef gtsam::PinholeCamera<gtsam::Cal3Bundler> PinholeCameraCal3Bundler;
|
||||
|
||||
template<T = {gtsam::PinholeCameraCal3Bundler, gtsam::PinholeCameraCal3_S2}>
|
||||
|
|
@ -2091,7 +2050,7 @@ class NonlinearFactorGraph {
|
|||
gtsam::KeySet keys() const;
|
||||
gtsam::KeyVector keyVector() const;
|
||||
|
||||
template<T = {Vector, gtsam::Point2, gtsam::StereoPoint2, gtsam::Point3, gtsam::Rot2, gtsam::SO3, gtsam::SO4, gtsam::Rot3, gtsam::Pose2, gtsam::Pose3, gtsam::Cal3_S2,gtsam::CalibratedCamera, gtsam::SimpleCamera, gtsam::PinholeCameraCal3_S2, gtsam::PinholeCamera<gtsam::Cal3Bundler>, gtsam::imuBias::ConstantBias}>
|
||||
template<T = {Vector, gtsam::Point2, gtsam::StereoPoint2, gtsam::Point3, gtsam::Rot2, gtsam::SO3, gtsam::SO4, gtsam::Rot3, gtsam::Pose2, gtsam::Pose3, gtsam::Cal3_S2,gtsam::CalibratedCamera, gtsam::PinholeCameraCal3_S2, gtsam::PinholeCamera<gtsam::Cal3Bundler>, gtsam::imuBias::ConstantBias}>
|
||||
void addPrior(size_t key, const T& prior, const gtsam::noiseModel::Base* noiseModel);
|
||||
|
||||
// NonlinearFactorGraph
|
||||
|
|
@ -2515,7 +2474,7 @@ class ISAM2 {
|
|||
template <VALUE = {gtsam::Point2, gtsam::Rot2, gtsam::Pose2, gtsam::Point3,
|
||||
gtsam::Rot3, gtsam::Pose3, gtsam::Cal3_S2, gtsam::Cal3DS2,
|
||||
gtsam::Cal3Bundler, gtsam::EssentialMatrix,
|
||||
gtsam::SimpleCamera, gtsam::PinholeCameraCal3_S2, gtsam::PinholeCamera<gtsam::Cal3Bundler>,
|
||||
gtsam::PinholeCameraCal3_S2, gtsam::PinholeCamera<gtsam::Cal3Bundler>,
|
||||
Vector, Matrix}>
|
||||
VALUE calculateEstimate(size_t key) const;
|
||||
gtsam::Values calculateBestEstimate() const;
|
||||
|
|
@ -2549,12 +2508,11 @@ class NonlinearISAM {
|
|||
//*************************************************************************
|
||||
// Nonlinear factor types
|
||||
//*************************************************************************
|
||||
#include <gtsam/geometry/SimpleCamera.h>
|
||||
#include <gtsam/geometry/CalibratedCamera.h>
|
||||
#include <gtsam/geometry/StereoPoint2.h>
|
||||
|
||||
#include <gtsam/nonlinear/PriorFactor.h>
|
||||
template<T = {Vector, gtsam::Point2, gtsam::StereoPoint2, gtsam::Point3, gtsam::Rot2, gtsam::SO3, gtsam::SO4, gtsam::SOn, gtsam::Rot3, gtsam::Pose2, gtsam::Pose3, gtsam::Unit3, gtsam::Cal3_S2,gtsam::CalibratedCamera, gtsam::SimpleCamera, gtsam::PinholeCameraCal3_S2, gtsam::imuBias::ConstantBias, gtsam::PinholeCamera<gtsam::Cal3Bundler>}>
|
||||
template<T = {Vector, gtsam::Point2, gtsam::StereoPoint2, gtsam::Point3, gtsam::Rot2, gtsam::SO3, gtsam::SO4, gtsam::SOn, gtsam::Rot3, gtsam::Pose2, gtsam::Pose3, gtsam::Unit3, gtsam::Cal3_S2,gtsam::CalibratedCamera, gtsam::PinholeCameraCal3_S2, gtsam::imuBias::ConstantBias, gtsam::PinholeCamera<gtsam::Cal3Bundler>}>
|
||||
virtual class PriorFactor : gtsam::NoiseModelFactor {
|
||||
PriorFactor(size_t key, const T& prior, const gtsam::noiseModel::Base* noiseModel);
|
||||
T prior() const;
|
||||
|
|
@ -2578,7 +2536,7 @@ virtual class BetweenFactor : gtsam::NoiseModelFactor {
|
|||
template <T = {gtsam::Point2, gtsam::StereoPoint2, gtsam::Point3, gtsam::Rot2,
|
||||
gtsam::SO3, gtsam::SO4, gtsam::SOn, gtsam::Rot3, gtsam::Pose2,
|
||||
gtsam::Pose3, gtsam::Cal3_S2, gtsam::CalibratedCamera,
|
||||
gtsam::SimpleCamera, gtsam::PinholeCameraCal3_S2,
|
||||
gtsam::PinholeCameraCal3_S2,
|
||||
gtsam::imuBias::ConstantBias}>
|
||||
virtual class NonlinearEquality : gtsam::NoiseModelFactor {
|
||||
// Constructor - forces exact evaluation
|
||||
|
|
@ -2697,8 +2655,7 @@ virtual class GeneralSFMFactor : gtsam::NoiseModelFactor {
|
|||
gtsam::Point2 measured() const;
|
||||
};
|
||||
typedef gtsam::GeneralSFMFactor<gtsam::PinholeCameraCal3_S2, gtsam::Point3> GeneralSFMFactorCal3_S2;
|
||||
//TODO (Issue 237) due to lack of jacobians of Cal3DS2_Base::calibrate, GeneralSFMFactor does not apply to Cal3DS2
|
||||
//typedef gtsam::GeneralSFMFactor<gtsam::PinholeCameraCal3DS2, gtsam::Point3> GeneralSFMFactorCal3DS2;
|
||||
typedef gtsam::GeneralSFMFactor<gtsam::PinholeCameraCal3DS2, gtsam::Point3> GeneralSFMFactorCal3DS2;
|
||||
typedef gtsam::GeneralSFMFactor<gtsam::PinholeCamera<gtsam::Cal3Bundler>, gtsam::Point3> GeneralSFMFactorCal3Bundler;
|
||||
|
||||
template<CALIBRATION = {gtsam::Cal3_S2}>
|
||||
|
|
|
|||
|
|
@ -383,7 +383,7 @@ Subgraph SubgraphBuilder::operator()(const GaussianFactorGraph &gfg) const {
|
|||
const vector<size_t> tree = buildTree(gfg, forward_ordering, weights);
|
||||
if (tree.size() != n - 1) {
|
||||
throw std::runtime_error(
|
||||
"SubgraphBuilder::operator() failure: tree.size() != n-1");
|
||||
"SubgraphBuilder::operator() failure: tree.size() != n-1, might be caused by disconnected graph");
|
||||
}
|
||||
|
||||
// Downweight the tree edges to zero.
|
||||
|
|
|
|||
|
|
@ -29,8 +29,8 @@ namespace imuBias {
|
|||
|
||||
class GTSAM_EXPORT ConstantBias {
|
||||
private:
|
||||
Vector3 biasAcc_;
|
||||
Vector3 biasGyro_;
|
||||
Vector3 biasAcc_; ///< The units for stddev are σ = m/s² or m √Hz/s²
|
||||
Vector3 biasGyro_; ///< The units for stddev are σ = rad/s or rad √Hz/s
|
||||
|
||||
public:
|
||||
/// dimension of the variable - used to autodetect sizes
|
||||
|
|
|
|||
|
|
@ -29,7 +29,9 @@ namespace gtsam {
|
|||
/// Parameters for pre-integration:
|
||||
/// Usage: Create just a single Params and pass a shared pointer to the constructor
|
||||
struct GTSAM_EXPORT PreintegratedRotationParams {
|
||||
Matrix3 gyroscopeCovariance; ///< continuous-time "Covariance" of gyroscope measurements
|
||||
/// Continuous-time "Covariance" of gyroscope measurements
|
||||
/// The units for stddev are σ = rad/s/√Hz
|
||||
Matrix3 gyroscopeCovariance;
|
||||
boost::optional<Vector3> omegaCoriolis; ///< Coriolis constant
|
||||
boost::optional<Pose3> body_P_sensor; ///< The pose of the sensor in the body frame
|
||||
|
||||
|
|
|
|||
|
|
@ -24,7 +24,9 @@ namespace gtsam {
|
|||
/// Parameters for pre-integration:
|
||||
/// Usage: Create just a single Params and pass a shared pointer to the constructor
|
||||
struct GTSAM_EXPORT PreintegrationParams: PreintegratedRotationParams {
|
||||
Matrix3 accelerometerCovariance; ///< continuous-time "Covariance" of accelerometer
|
||||
/// Continuous-time "Covariance" of accelerometer
|
||||
/// The units for stddev are σ = m/s²/√Hz
|
||||
Matrix3 accelerometerCovariance;
|
||||
Matrix3 integrationCovariance; ///< continuous-time "Covariance" describing integration uncertainty
|
||||
bool use2ndOrderCoriolis; ///< Whether to use second order Coriolis integration
|
||||
Vector3 n_gravity; ///< Gravity vector in nav frame
|
||||
|
|
|
|||
|
|
@ -338,19 +338,18 @@ namespace gtsam {
|
|||
} // internal
|
||||
|
||||
/* ************************************************************************* */
|
||||
template<typename ValueType>
|
||||
ValueType Values::at(Key j) const {
|
||||
template <typename ValueType>
|
||||
const ValueType Values::at(Key j) const {
|
||||
// Find the item
|
||||
KeyValueMap::const_iterator item = values_.find(j);
|
||||
|
||||
// Throw exception if it does not exist
|
||||
if(item == values_.end())
|
||||
throw ValuesKeyDoesNotExist("at", j);
|
||||
if (item == values_.end()) throw ValuesKeyDoesNotExist("at", j);
|
||||
|
||||
// Check the type and throw exception if incorrect
|
||||
// h() split in two lines to avoid internal compiler error (MSVC2017)
|
||||
auto h = internal::handle<ValueType>();
|
||||
return h(j,item->second);
|
||||
// Check the type and throw exception if incorrect
|
||||
// h() split in two lines to avoid internal compiler error (MSVC2017)
|
||||
auto h = internal::handle<ValueType>();
|
||||
return h(j, item->second);
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
|
|
|
|||
|
|
@ -187,8 +187,8 @@ namespace gtsam {
|
|||
* Dynamic matrices/vectors can be retrieved as fixed-size, but not vice-versa.
|
||||
* @return The stored value
|
||||
*/
|
||||
template<typename ValueType>
|
||||
ValueType at(Key j) const;
|
||||
template <typename ValueType>
|
||||
const ValueType at(Key j) const;
|
||||
|
||||
/// version for double
|
||||
double atDouble(size_t key) const { return at<double>(key);}
|
||||
|
|
|
|||
|
|
@ -91,6 +91,27 @@ TEST(ShonanAveraging3, checkOptimality) {
|
|||
EXPECT(!kShonan.checkOptimality(random));
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
TEST(ShonanAveraging3, checkSubgraph) {
|
||||
// Create parameter with solver set to SUBGRAPH
|
||||
auto params = ShonanAveragingParameters3(
|
||||
gtsam::LevenbergMarquardtParams::CeresDefaults(), "SUBGRAPH");
|
||||
ShonanAveraging3::Measurements measurements;
|
||||
|
||||
// The toyExample.g2o has 5 vertices, from 0-4
|
||||
// The edges are: 1-2, 2-3, 3-4, 3-1, 1-4, 0-1,
|
||||
// which can build a connected graph
|
||||
auto subgraphShonan = fromExampleName("toyExample.g2o", params);
|
||||
|
||||
// Create initial random estimation
|
||||
Values initial;
|
||||
initial = subgraphShonan.initializeRandomly(kRandomNumberGenerator);
|
||||
|
||||
// Run Shonan with SUBGRAPH solver
|
||||
auto result = subgraphShonan.run(initial, 3, 3);
|
||||
EXPECT_DOUBLES_EQUAL(1e-11, subgraphShonan.cost(result.first), 1e-4);
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
TEST(ShonanAveraging3, tryOptimizingAt3) {
|
||||
const Values randomRotations = kShonan.initializeRandomly(kRandomNumberGenerator);
|
||||
|
|
|
|||
|
|
@ -43,7 +43,6 @@ typedef PriorFactor<Pose3> PriorFactorPose3;
|
|||
typedef PriorFactor<Cal3_S2> PriorFactorCal3_S2;
|
||||
typedef PriorFactor<Cal3DS2> PriorFactorCal3DS2;
|
||||
typedef PriorFactor<CalibratedCamera> PriorFactorCalibratedCamera;
|
||||
typedef PriorFactor<SimpleCamera> PriorFactorSimpleCamera;
|
||||
typedef PriorFactor<PinholeCameraCal3_S2> PriorFactorPinholeCameraCal3_S2;
|
||||
typedef PriorFactor<StereoCamera> PriorFactorStereoCamera;
|
||||
|
||||
|
|
@ -68,7 +67,6 @@ typedef NonlinearEquality<Pose3> NonlinearEqualityPose3;
|
|||
typedef NonlinearEquality<Cal3_S2> NonlinearEqualityCal3_S2;
|
||||
typedef NonlinearEquality<Cal3DS2> NonlinearEqualityCal3DS2;
|
||||
typedef NonlinearEquality<CalibratedCamera> NonlinearEqualityCalibratedCamera;
|
||||
typedef NonlinearEquality<SimpleCamera> NonlinearEqualitySimpleCamera;
|
||||
typedef NonlinearEquality<PinholeCameraCal3_S2> NonlinearEqualityPinholeCameraCal3_S2;
|
||||
typedef NonlinearEquality<StereoCamera> NonlinearEqualityStereoCamera;
|
||||
|
||||
|
|
@ -77,10 +75,8 @@ typedef RangeFactor<Pose3, Point3> RangeFactor3D;
|
|||
typedef RangeFactor<Pose2, Pose2> RangeFactorPose2;
|
||||
typedef RangeFactor<Pose3, Pose3> RangeFactorPose3;
|
||||
typedef RangeFactor<CalibratedCamera, Point3> RangeFactorCalibratedCameraPoint;
|
||||
typedef RangeFactor<SimpleCamera, Point3> RangeFactorSimpleCameraPoint;
|
||||
typedef RangeFactor<PinholeCameraCal3_S2, Point3> RangeFactorPinholeCameraCal3_S2Point;
|
||||
typedef RangeFactor<CalibratedCamera, CalibratedCamera> RangeFactorCalibratedCamera;
|
||||
typedef RangeFactor<SimpleCamera, SimpleCamera> RangeFactorSimpleCamera;
|
||||
typedef RangeFactor<PinholeCameraCal3_S2, PinholeCameraCal3_S2> RangeFactorPinholeCameraCal3_S2;
|
||||
|
||||
typedef BearingRangeFactor<Pose2, Point2> BearingRangeFactor2D;
|
||||
|
|
@ -90,7 +86,7 @@ typedef GenericProjectionFactor<Pose3, Point3, Cal3_S2> GenericProjectionFactorC
|
|||
typedef GenericProjectionFactor<Pose3, Point3, Cal3DS2> GenericProjectionFactorCal3DS2;
|
||||
|
||||
typedef gtsam::GeneralSFMFactor<gtsam::PinholeCameraCal3_S2, gtsam::Point3> GeneralSFMFactorCal3_S2;
|
||||
//typedef gtsam::GeneralSFMFactor<gtsam::PinholeCameraCal3DS2, gtsam::Point3> GeneralSFMFactorCal3DS2;
|
||||
typedef gtsam::GeneralSFMFactor<gtsam::PinholeCameraCal3DS2, gtsam::Point3> GeneralSFMFactorCal3DS2;
|
||||
|
||||
typedef gtsam::GeneralSFMFactor2<gtsam::Cal3_S2> GeneralSFMFactor2Cal3_S2;
|
||||
|
||||
|
|
@ -129,7 +125,6 @@ GTSAM_VALUE_EXPORT(gtsam::Cal3_S2);
|
|||
GTSAM_VALUE_EXPORT(gtsam::Cal3DS2);
|
||||
GTSAM_VALUE_EXPORT(gtsam::Cal3_S2Stereo);
|
||||
GTSAM_VALUE_EXPORT(gtsam::CalibratedCamera);
|
||||
GTSAM_VALUE_EXPORT(gtsam::SimpleCamera);
|
||||
GTSAM_VALUE_EXPORT(gtsam::PinholeCameraCal3_S2);
|
||||
GTSAM_VALUE_EXPORT(gtsam::StereoCamera);
|
||||
|
||||
|
|
@ -150,7 +145,6 @@ BOOST_CLASS_EXPORT_GUID(PriorFactorPose3, "gtsam::PriorFactorPose3");
|
|||
BOOST_CLASS_EXPORT_GUID(PriorFactorCal3_S2, "gtsam::PriorFactorCal3_S2");
|
||||
BOOST_CLASS_EXPORT_GUID(PriorFactorCal3DS2, "gtsam::PriorFactorCal3DS2");
|
||||
BOOST_CLASS_EXPORT_GUID(PriorFactorCalibratedCamera, "gtsam::PriorFactorCalibratedCamera");
|
||||
BOOST_CLASS_EXPORT_GUID(PriorFactorSimpleCamera, "gtsam::PriorFactorSimpleCamera");
|
||||
BOOST_CLASS_EXPORT_GUID(PriorFactorStereoCamera, "gtsam::PriorFactorStereoCamera");
|
||||
|
||||
BOOST_CLASS_EXPORT_GUID(BetweenFactorLieVector, "gtsam::BetweenFactorLieVector");
|
||||
|
|
@ -174,7 +168,6 @@ BOOST_CLASS_EXPORT_GUID(NonlinearEqualityPose3, "gtsam::NonlinearEqualityPose3")
|
|||
BOOST_CLASS_EXPORT_GUID(NonlinearEqualityCal3_S2, "gtsam::NonlinearEqualityCal3_S2");
|
||||
BOOST_CLASS_EXPORT_GUID(NonlinearEqualityCal3DS2, "gtsam::NonlinearEqualityCal3DS2");
|
||||
BOOST_CLASS_EXPORT_GUID(NonlinearEqualityCalibratedCamera, "gtsam::NonlinearEqualityCalibratedCamera");
|
||||
BOOST_CLASS_EXPORT_GUID(NonlinearEqualitySimpleCamera, "gtsam::NonlinearEqualitySimpleCamera");
|
||||
BOOST_CLASS_EXPORT_GUID(NonlinearEqualityStereoCamera, "gtsam::NonlinearEqualityStereoCamera");
|
||||
|
||||
BOOST_CLASS_EXPORT_GUID(RangeFactor2D, "gtsam::RangeFactor2D");
|
||||
|
|
@ -182,9 +175,7 @@ BOOST_CLASS_EXPORT_GUID(RangeFactor3D, "gtsam::RangeFactor3D");
|
|||
BOOST_CLASS_EXPORT_GUID(RangeFactorPose2, "gtsam::RangeFactorPose2");
|
||||
BOOST_CLASS_EXPORT_GUID(RangeFactorPose3, "gtsam::RangeFactorPose3");
|
||||
BOOST_CLASS_EXPORT_GUID(RangeFactorCalibratedCameraPoint, "gtsam::RangeFactorCalibratedCameraPoint");
|
||||
BOOST_CLASS_EXPORT_GUID(RangeFactorSimpleCameraPoint, "gtsam::RangeFactorSimpleCameraPoint");
|
||||
BOOST_CLASS_EXPORT_GUID(RangeFactorCalibratedCamera, "gtsam::RangeFactorCalibratedCamera");
|
||||
BOOST_CLASS_EXPORT_GUID(RangeFactorSimpleCamera, "gtsam::RangeFactorSimpleCamera");
|
||||
|
||||
BOOST_CLASS_EXPORT_GUID(BearingRangeFactor2D, "gtsam::BearingRangeFactor2D");
|
||||
|
||||
|
|
@ -192,12 +183,28 @@ BOOST_CLASS_EXPORT_GUID(GenericProjectionFactorCal3_S2, "gtsam::GenericProjectio
|
|||
BOOST_CLASS_EXPORT_GUID(GenericProjectionFactorCal3DS2, "gtsam::GenericProjectionFactorCal3DS2");
|
||||
|
||||
BOOST_CLASS_EXPORT_GUID(GeneralSFMFactorCal3_S2, "gtsam::GeneralSFMFactorCal3_S2");
|
||||
//BOOST_CLASS_EXPORT_GUID(GeneralSFMFactorCal3DS2, "gtsam::GeneralSFMFactorCal3DS2");
|
||||
BOOST_CLASS_EXPORT_GUID(GeneralSFMFactorCal3DS2, "gtsam::GeneralSFMFactorCal3DS2");
|
||||
|
||||
BOOST_CLASS_EXPORT_GUID(GeneralSFMFactor2Cal3_S2, "gtsam::GeneralSFMFactor2Cal3_S2");
|
||||
|
||||
BOOST_CLASS_EXPORT_GUID(GenericStereoFactor3D, "gtsam::GenericStereoFactor3D");
|
||||
|
||||
#ifdef GTSAM_ALLOW_DEPRECATED_SINCE_V41
|
||||
|
||||
typedef PriorFactor<SimpleCamera> PriorFactorSimpleCamera;
|
||||
typedef NonlinearEquality<SimpleCamera> NonlinearEqualitySimpleCamera;
|
||||
typedef RangeFactor<SimpleCamera, Point3> RangeFactorSimpleCameraPoint;
|
||||
typedef RangeFactor<SimpleCamera, SimpleCamera> RangeFactorSimpleCamera;
|
||||
|
||||
GTSAM_VALUE_EXPORT(gtsam::SimpleCamera);
|
||||
BOOST_CLASS_EXPORT_GUID(PriorFactorSimpleCamera, "gtsam::PriorFactorSimpleCamera");
|
||||
BOOST_CLASS_EXPORT_GUID(NonlinearEqualitySimpleCamera, "gtsam::NonlinearEqualitySimpleCamera");
|
||||
BOOST_CLASS_EXPORT_GUID(RangeFactorSimpleCameraPoint, "gtsam::RangeFactorSimpleCameraPoint");
|
||||
BOOST_CLASS_EXPORT_GUID(RangeFactorSimpleCamera, "gtsam::RangeFactorSimpleCamera");
|
||||
|
||||
#endif
|
||||
|
||||
|
||||
/* ************************************************************************* */
|
||||
// Actual implementations of functions
|
||||
/* ************************************************************************* */
|
||||
|
|
|
|||
|
|
@ -14,11 +14,12 @@ import unittest
|
|||
import numpy as np
|
||||
|
||||
import gtsam
|
||||
from gtsam import Cal3_S2, Point3, Pose2, Pose3, Rot3, SimpleCamera
|
||||
from gtsam import Cal3_S2, Point3, Pose2, Pose3, Rot3, PinholeCameraCal3_S2 as SimpleCamera
|
||||
from gtsam.utils.test_case import GtsamTestCase
|
||||
|
||||
K = Cal3_S2(625, 625, 0, 0, 0)
|
||||
|
||||
|
||||
class TestSimpleCamera(GtsamTestCase):
|
||||
|
||||
def test_constructor(self):
|
||||
|
|
@ -29,15 +30,15 @@ class TestSimpleCamera(GtsamTestCase):
|
|||
|
||||
def test_level2(self):
|
||||
# Create a level camera, looking in Y-direction
|
||||
pose2 = Pose2(0.4,0.3,math.pi/2.0)
|
||||
pose2 = Pose2(0.4, 0.3, math.pi/2.0)
|
||||
camera = SimpleCamera.Level(K, pose2, 0.1)
|
||||
|
||||
# expected
|
||||
x = Point3(1,0,0)
|
||||
y = Point3(0,0,-1)
|
||||
z = Point3(0,1,0)
|
||||
wRc = Rot3(x,y,z)
|
||||
expected = Pose3(wRc,Point3(0.4,0.3,0.1))
|
||||
x = Point3(1, 0, 0)
|
||||
y = Point3(0, 0, -1)
|
||||
z = Point3(0, 1, 0)
|
||||
wRc = Rot3(x, y, z)
|
||||
expected = Pose3(wRc, Point3(0.4, 0.3, 0.1))
|
||||
self.gtsamAssertEquals(camera.pose(), expected, 1e-9)
|
||||
|
||||
|
||||
|
|
|
|||
|
|
@ -36,18 +36,15 @@ def set_axes_equal(fignum):
|
|||
ax.set_zlim3d([origin[2] - radius, origin[2] + radius])
|
||||
|
||||
|
||||
def ellipsoid(xc, yc, zc, rx, ry, rz, n):
|
||||
def ellipsoid(rx, ry, rz, n):
|
||||
"""
|
||||
Numpy equivalent of Matlab's ellipsoid function.
|
||||
|
||||
Args:
|
||||
xc (double): Center of ellipsoid in X-axis.
|
||||
yc (double): Center of ellipsoid in Y-axis.
|
||||
zc (double): Center of ellipsoid in Z-axis.
|
||||
rx (double): Radius of ellipsoid in X-axis.
|
||||
ry (double): Radius of ellipsoid in Y-axis.
|
||||
rz (double): Radius of ellipsoid in Z-axis.
|
||||
n (int): The granularity of the ellipsoid plotted.
|
||||
n (int): The granularity of the ellipsoid plotted.
|
||||
|
||||
Returns:
|
||||
tuple[numpy.ndarray]: The points in the x, y and z axes to use for the surface plot.
|
||||
|
|
@ -72,7 +69,8 @@ def plot_covariance_ellipse_3d(axes, origin, P, scale=1, n=8, alpha=0.5):
|
|||
Args:
|
||||
axes (matplotlib.axes.Axes): Matplotlib axes.
|
||||
origin (gtsam.Point3): The origin in the world frame.
|
||||
P (numpy.ndarray): The marginal covariance matrix of the 3D point which will be represented as an ellipse.
|
||||
P (numpy.ndarray): The marginal covariance matrix of the 3D point
|
||||
which will be represented as an ellipse.
|
||||
scale (float): Scaling factor of the radii of the covariance ellipse.
|
||||
n (int): Defines the granularity of the ellipse. Higher values indicate finer ellipses.
|
||||
alpha (float): Transparency value for the plotted surface in the range [0, 1].
|
||||
|
|
@ -85,7 +83,7 @@ def plot_covariance_ellipse_3d(axes, origin, P, scale=1, n=8, alpha=0.5):
|
|||
rx, ry, rz = radii
|
||||
|
||||
# generate data for "unrotated" ellipsoid
|
||||
xc, yc, zc = ellipsoid(0, 0, 0, rx, ry, rz, n)
|
||||
xc, yc, zc = ellipsoid(rx, ry, rz, n)
|
||||
|
||||
# rotate data with orientation matrix U and center c
|
||||
data = np.kron(U[:, 0:1], xc) + np.kron(U[:, 1:2], yc) + \
|
||||
|
|
@ -106,7 +104,8 @@ def plot_pose2_on_axes(axes, pose, axis_length=0.1, covariance=None):
|
|||
axes (matplotlib.axes.Axes): Matplotlib axes.
|
||||
pose (gtsam.Pose2): The pose to be plotted.
|
||||
axis_length (float): The length of the camera axes.
|
||||
covariance (numpy.ndarray): Marginal covariance matrix to plot the uncertainty of the estimation.
|
||||
covariance (numpy.ndarray): Marginal covariance matrix to plot
|
||||
the uncertainty of the estimation.
|
||||
"""
|
||||
# get rotation and translation (center)
|
||||
gRp = pose.rotation().matrix() # rotation from pose to global
|
||||
|
|
@ -146,7 +145,8 @@ def plot_pose2(fignum, pose, axis_length=0.1, covariance=None,
|
|||
fignum (int): Integer representing the figure number to use for plotting.
|
||||
pose (gtsam.Pose2): The pose to be plotted.
|
||||
axis_length (float): The length of the camera axes.
|
||||
covariance (numpy.ndarray): Marginal covariance matrix to plot the uncertainty of the estimation.
|
||||
covariance (numpy.ndarray): Marginal covariance matrix to plot
|
||||
the uncertainty of the estimation.
|
||||
axis_labels (iterable[string]): List of axis labels to set.
|
||||
"""
|
||||
# get figure object
|
||||
|
|
@ -215,7 +215,8 @@ def plot_3d_points(fignum, values, linespec="g*", marginals=None,
|
|||
fignum (int): Integer representing the figure number to use for plotting.
|
||||
values (gtsam.Values): Values dictionary consisting of points to be plotted.
|
||||
linespec (string): String representing formatting options for Matplotlib.
|
||||
marginals (numpy.ndarray): Marginal covariance matrix to plot the uncertainty of the estimation.
|
||||
marginals (numpy.ndarray): Marginal covariance matrix to plot the
|
||||
uncertainty of the estimation.
|
||||
title (string): The title of the plot.
|
||||
axis_labels (iterable[string]): List of axis labels to set.
|
||||
"""
|
||||
|
|
@ -238,6 +239,7 @@ def plot_3d_points(fignum, values, linespec="g*", marginals=None,
|
|||
continue
|
||||
# I guess it's not a Point3
|
||||
|
||||
fig = plt.figure(fignum)
|
||||
fig.suptitle(title)
|
||||
fig.canvas.set_window_title(title.lower())
|
||||
|
||||
|
|
|
|||
|
|
@ -89,10 +89,8 @@ typedef RangeFactor<Pose3, Point3> RangeFactor3D;
|
|||
typedef RangeFactor<Pose2, Pose2> RangeFactorPose2;
|
||||
typedef RangeFactor<Pose3, Pose3> RangeFactorPose3;
|
||||
typedef RangeFactor<CalibratedCamera, Point3> RangeFactorCalibratedCameraPoint;
|
||||
typedef RangeFactor<SimpleCamera, Point3> RangeFactorSimpleCameraPoint;
|
||||
typedef RangeFactor<PinholeCameraCal3_S2, Point3> RangeFactorPinholeCameraCal3_S2Point;
|
||||
typedef RangeFactor<CalibratedCamera, CalibratedCamera> RangeFactorCalibratedCamera;
|
||||
typedef RangeFactor<SimpleCamera, SimpleCamera> RangeFactorSimpleCamera;
|
||||
typedef RangeFactor<PinholeCameraCal3_S2, PinholeCameraCal3_S2> RangeFactorPinholeCameraCal3_S2;
|
||||
|
||||
typedef BearingRangeFactor<Pose2, Point2> BearingRangeFactor2D;
|
||||
|
|
@ -102,7 +100,7 @@ typedef GenericProjectionFactor<Pose3, Point3, Cal3_S2> GenericProjectionFactorC
|
|||
typedef GenericProjectionFactor<Pose3, Point3, Cal3DS2> GenericProjectionFactorCal3DS2;
|
||||
|
||||
typedef gtsam::GeneralSFMFactor<gtsam::PinholeCameraCal3_S2, gtsam::Point3> GeneralSFMFactorCal3_S2;
|
||||
//typedef gtsam::GeneralSFMFactor<gtsam::PinholeCameraCal3DS2, gtsam::Point3> GeneralSFMFactorCal3DS2;
|
||||
typedef gtsam::GeneralSFMFactor<gtsam::PinholeCameraCal3DS2, gtsam::Point3> GeneralSFMFactorCal3DS2;
|
||||
|
||||
typedef gtsam::GeneralSFMFactor2<gtsam::Cal3_S2> GeneralSFMFactor2Cal3_S2;
|
||||
|
||||
|
|
@ -145,7 +143,6 @@ GTSAM_VALUE_EXPORT(gtsam::Cal3_S2);
|
|||
GTSAM_VALUE_EXPORT(gtsam::Cal3DS2);
|
||||
GTSAM_VALUE_EXPORT(gtsam::Cal3_S2Stereo);
|
||||
GTSAM_VALUE_EXPORT(gtsam::CalibratedCamera);
|
||||
GTSAM_VALUE_EXPORT(gtsam::SimpleCamera);
|
||||
GTSAM_VALUE_EXPORT(gtsam::PinholeCameraCal3_S2);
|
||||
GTSAM_VALUE_EXPORT(gtsam::StereoCamera);
|
||||
|
||||
|
|
@ -190,9 +187,9 @@ BOOST_CLASS_EXPORT_GUID(RangeFactor3D, "gtsam::RangeFactor3D");
|
|||
BOOST_CLASS_EXPORT_GUID(RangeFactorPose2, "gtsam::RangeFactorPose2");
|
||||
BOOST_CLASS_EXPORT_GUID(RangeFactorPose3, "gtsam::RangeFactorPose3");
|
||||
BOOST_CLASS_EXPORT_GUID(RangeFactorCalibratedCameraPoint, "gtsam::RangeFactorCalibratedCameraPoint");
|
||||
BOOST_CLASS_EXPORT_GUID(RangeFactorSimpleCameraPoint, "gtsam::RangeFactorSimpleCameraPoint");
|
||||
BOOST_CLASS_EXPORT_GUID(RangeFactorPinholeCameraCal3_S2Point, "gtsam::RangeFactorPinholeCameraCal3_S2Point");
|
||||
BOOST_CLASS_EXPORT_GUID(RangeFactorCalibratedCamera, "gtsam::RangeFactorCalibratedCamera");
|
||||
BOOST_CLASS_EXPORT_GUID(RangeFactorSimpleCamera, "gtsam::RangeFactorSimpleCamera");
|
||||
BOOST_CLASS_EXPORT_GUID(RangeFactorPinholeCameraCal3_S2, "gtsam::RangeFactorPinholeCameraCal3_S2");
|
||||
|
||||
BOOST_CLASS_EXPORT_GUID(BearingRangeFactor2D, "gtsam::BearingRangeFactor2D");
|
||||
|
||||
|
|
@ -200,7 +197,7 @@ BOOST_CLASS_EXPORT_GUID(GenericProjectionFactorCal3_S2, "gtsam::GenericProjectio
|
|||
BOOST_CLASS_EXPORT_GUID(GenericProjectionFactorCal3DS2, "gtsam::GenericProjectionFactorCal3DS2");
|
||||
|
||||
BOOST_CLASS_EXPORT_GUID(GeneralSFMFactorCal3_S2, "gtsam::GeneralSFMFactorCal3_S2");
|
||||
//BOOST_CLASS_EXPORT_GUID(GeneralSFMFactorCal3DS2, "gtsam::GeneralSFMFactorCal3DS2");
|
||||
BOOST_CLASS_EXPORT_GUID(GeneralSFMFactorCal3DS2, "gtsam::GeneralSFMFactorCal3DS2");
|
||||
|
||||
BOOST_CLASS_EXPORT_GUID(GeneralSFMFactor2Cal3_S2, "gtsam::GeneralSFMFactor2Cal3_S2");
|
||||
|
||||
|
|
@ -352,9 +349,9 @@ TEST (testSerializationSLAM, factors) {
|
|||
RangeFactorPose2 rangeFactorPose2(a08, b08, 2.0, model1);
|
||||
RangeFactorPose3 rangeFactorPose3(a09, b09, 2.0, model1);
|
||||
RangeFactorCalibratedCameraPoint rangeFactorCalibratedCameraPoint(a12, a05, 2.0, model1);
|
||||
RangeFactorSimpleCameraPoint rangeFactorSimpleCameraPoint(a13, a05, 2.0, model1);
|
||||
RangeFactorPinholeCameraCal3_S2Point rangeFactorPinholeCameraCal3_S2Point(a13, a05, 2.0, model1);
|
||||
RangeFactorCalibratedCamera rangeFactorCalibratedCamera(a12, b12, 2.0, model1);
|
||||
RangeFactorSimpleCamera rangeFactorSimpleCamera(a13, b13, 2.0, model1);
|
||||
RangeFactorPinholeCameraCal3_S2 rangeFactorPinholeCameraCal3_S2(a13, b13, 2.0, model1);
|
||||
|
||||
BearingRangeFactor2D bearingRangeFactor2D(a08, a03, rot2, 2.0, model2);
|
||||
|
||||
|
|
@ -405,9 +402,9 @@ TEST (testSerializationSLAM, factors) {
|
|||
graph += rangeFactorPose2;
|
||||
graph += rangeFactorPose3;
|
||||
graph += rangeFactorCalibratedCameraPoint;
|
||||
graph += rangeFactorSimpleCameraPoint;
|
||||
graph += rangeFactorPinholeCameraCal3_S2Point;
|
||||
graph += rangeFactorCalibratedCamera;
|
||||
graph += rangeFactorSimpleCamera;
|
||||
graph += rangeFactorPinholeCameraCal3_S2;
|
||||
|
||||
graph += bearingRangeFactor2D;
|
||||
|
||||
|
|
@ -463,9 +460,9 @@ TEST (testSerializationSLAM, factors) {
|
|||
EXPECT(equalsObj<RangeFactorPose2>(rangeFactorPose2));
|
||||
EXPECT(equalsObj<RangeFactorPose3>(rangeFactorPose3));
|
||||
EXPECT(equalsObj<RangeFactorCalibratedCameraPoint>(rangeFactorCalibratedCameraPoint));
|
||||
EXPECT(equalsObj<RangeFactorSimpleCameraPoint>(rangeFactorSimpleCameraPoint));
|
||||
EXPECT(equalsObj<RangeFactorPinholeCameraCal3_S2Point>(rangeFactorPinholeCameraCal3_S2Point));
|
||||
EXPECT(equalsObj<RangeFactorCalibratedCamera>(rangeFactorCalibratedCamera));
|
||||
EXPECT(equalsObj<RangeFactorSimpleCamera>(rangeFactorSimpleCamera));
|
||||
EXPECT(equalsObj<RangeFactorPinholeCameraCal3_S2>(rangeFactorPinholeCameraCal3_S2));
|
||||
|
||||
EXPECT(equalsObj<BearingRangeFactor2D>(bearingRangeFactor2D));
|
||||
|
||||
|
|
@ -521,9 +518,9 @@ TEST (testSerializationSLAM, factors) {
|
|||
EXPECT(equalsXML<RangeFactorPose2>(rangeFactorPose2));
|
||||
EXPECT(equalsXML<RangeFactorPose3>(rangeFactorPose3));
|
||||
EXPECT(equalsXML<RangeFactorCalibratedCameraPoint>(rangeFactorCalibratedCameraPoint));
|
||||
EXPECT(equalsXML<RangeFactorSimpleCameraPoint>(rangeFactorSimpleCameraPoint));
|
||||
EXPECT(equalsXML<RangeFactorPinholeCameraCal3_S2Point>(rangeFactorPinholeCameraCal3_S2Point));
|
||||
EXPECT(equalsXML<RangeFactorCalibratedCamera>(rangeFactorCalibratedCamera));
|
||||
EXPECT(equalsXML<RangeFactorSimpleCamera>(rangeFactorSimpleCamera));
|
||||
EXPECT(equalsXML<RangeFactorPinholeCameraCal3_S2>(rangeFactorPinholeCameraCal3_S2));
|
||||
|
||||
EXPECT(equalsXML<BearingRangeFactor2D>(bearingRangeFactor2D));
|
||||
|
||||
|
|
@ -579,9 +576,9 @@ TEST (testSerializationSLAM, factors) {
|
|||
EXPECT(equalsBinary<RangeFactorPose2>(rangeFactorPose2));
|
||||
EXPECT(equalsBinary<RangeFactorPose3>(rangeFactorPose3));
|
||||
EXPECT(equalsBinary<RangeFactorCalibratedCameraPoint>(rangeFactorCalibratedCameraPoint));
|
||||
EXPECT(equalsBinary<RangeFactorSimpleCameraPoint>(rangeFactorSimpleCameraPoint));
|
||||
EXPECT(equalsBinary<RangeFactorPinholeCameraCal3_S2Point>(rangeFactorPinholeCameraCal3_S2Point));
|
||||
EXPECT(equalsBinary<RangeFactorCalibratedCamera>(rangeFactorCalibratedCamera));
|
||||
EXPECT(equalsBinary<RangeFactorSimpleCamera>(rangeFactorSimpleCamera));
|
||||
EXPECT(equalsBinary<RangeFactorPinholeCameraCal3_S2>(rangeFactorPinholeCameraCal3_S2));
|
||||
|
||||
EXPECT(equalsBinary<BearingRangeFactor2D>(bearingRangeFactor2D));
|
||||
|
||||
|
|
|
|||
Loading…
Reference in New Issue