diff --git a/README.md b/README.md index 4e9e1e529..9a87e5f6d 100644 --- a/README.md +++ b/README.md @@ -7,6 +7,9 @@ Kalibr is a toolbox that solves the following calibration problems: intrinsic and extrinsic calibration of a camera-systems with non-globally shared overlapping fields of view 1. **Camera-IMU calibration**: spatial and temporal calibration of an IMU w.r.t a camera-system +1. **Rolling Shutter Camera calibration**: + full intrinsic calibration (projection, distortion and shutter parameters) of rolling shutter cameras + **Please find more information on the [wiki pages](https://github.com/ethz-asl/kalibr/wiki) of this repository.** @@ -17,6 +20,7 @@ Kalibr is a toolbox that solves the following calibration problems: * Jérôme Maye ([email](jerome.maye@mavt.ethz.ch)) * Jörn Rehder ([email](joern.rehder@mavt.ethz.ch)) * Thomas Schneider ([email](schneith@ethz.ch)) +* Luc Oth ## References The calibration approaches used in Kalibr are based on the following papers. Please cite the appropriate papers when using this toolbox or parts of it in an academic publication. @@ -24,6 +28,7 @@ The calibration approaches used in Kalibr are based on the following papers. Ple 1. Paul Furgale, Joern Rehder, Roland Siegwart (2013). Unified Temporal and Spatial Calibration for Multi-Sensor Systems. In Proceedings of the IEEE/RSJ International Conference on Intelligent Robots and Systems (IROS), Tokyo, Japan. 1. Paul Furgale, T D Barfoot, G Sibley (2012). Continuous-Time Batch Estimation Using Temporal Basis Functions. In Proceedings of the IEEE International Conference on Robotics and Automation (ICRA), pp. 2088–2095, St. Paul, MN. 1. J. Maye, P. Furgale, R. Siegwart (2013). Self-supervised Calibration for Robotic Systems, In Proc. of the IEEE Intelligent Vehicles Symposium (IVS) +1. L. Oth, P. Furgale, L. Kneip, R. Siegwart (2013). Rolling Shutter Camera Calibration, In Proc. of the IEEE Computer Vision and Pattern Recognition (CVPR) ## Acknowledgments This work is supported in part by the European Community's Seventh Framework Programme (FP7/2007-2013) under grants #269916 (V-Charge), and #610603 (EUROPA2). diff --git a/Schweizer-Messer/sm_boost/include/boost/portable_binary_archive.hpp b/Schweizer-Messer/sm_boost/include/boost/portable_binary_archive.hpp index 65b3efbbf..4efa42c44 100755 --- a/Schweizer-Messer/sm_boost/include/boost/portable_binary_archive.hpp +++ b/Schweizer-Messer/sm_boost/include/boost/portable_binary_archive.hpp @@ -1,7 +1,7 @@ #ifndef PORTABLE_BINARY_ARCHIVE_HPP #define PORTABLE_BINARY_ARCHIVE_HPP -// (C) Copyright 2002 Robert Ramey - http://www.rrsd.com . +// (C) Copyright 2002 Robert Ramey - http://www.rrsd.com . // Use, modification and distribution is subject to the Boost Software // License, Version 1.0. (See accompanying file LICENSE_1_0.txt or copy at // http://www.boost.org/LICENSE_1_0.txt) @@ -13,7 +13,13 @@ #include #include + +// breaking changes in boost >=1.59 +#if BOOST_VERSION >= 105900 +#else #include +#endif + #include #include @@ -47,7 +53,6 @@ reverse_bytes(char size, char *address){ } } - } // namespace archive } // namespace boost diff --git a/Schweizer-Messer/sm_boost/include/boost/portable_binary_iarchive.hpp b/Schweizer-Messer/sm_boost/include/boost/portable_binary_iarchive.hpp index 93b7b06a6..c8aa12995 100755 --- a/Schweizer-Messer/sm_boost/include/boost/portable_binary_iarchive.hpp +++ b/Schweizer-Messer/sm_boost/include/boost/portable_binary_iarchive.hpp @@ -14,7 +14,7 @@ /////////1/////////2/////////3/////////4/////////5/////////6/////////7/////////8 // portable_binary_iarchive.hpp -// (C) Copyright 2002-7 Robert Ramey - http://www.rrsd.com . +// (C) Copyright 2002-7 Robert Ramey - http://www.rrsd.com . // Use, modification and distribution is subject to the Boost Software // License, Version 1.0. (See accompanying file LICENSE_1_0.txt or copy at // http://www.boost.org/LICENSE_1_0.txt) @@ -28,26 +28,28 @@ #include #include #include + #if BOOST_VERSION >= 105600 // see changes in https://github.com/boostorg/serialization/commit/75f09afc895ab09c4eb55d36fcf6f91ef4a0107a #include #else #include #endif + #include #include "portable_binary_archive.hpp" namespace boost { namespace archive { - + /////////1/////////2/////////3/////////4/////////5/////////6/////////7/////////8 // exception to be thrown if integer read from archive doesn't fit // variable being loaded -class portable_binary_iarchive_exception : +class portable_binary_iarchive_exception : public virtual boost::archive::archive_exception { public: typedef enum { - incompatible_integer_size + incompatible_integer_size } exception_code; portable_binary_iarchive_exception(exception_code /* c */ ) : boost::archive::archive_exception(boost::archive::archive_exception::other_exception) @@ -73,13 +75,13 @@ class portable_binary_iarchive_exception : }; /////////1/////////2/////////3/////////4/////////5/////////6/////////7/////////8 -// "Portable" input binary archive. It addresses integer size and endienness so +// "Portable" input binary archive. It addresses integer size and endienness so // that binary archives can be passed across systems. Note:floating point types // not addressed here class portable_binary_iarchive : public boost::archive::basic_binary_iprimitive< portable_binary_iarchive, - std::istream::char_type, + std::istream::char_type, std::istream::traits_type >, public boost::archive::detail::common_iarchive< @@ -93,7 +95,7 @@ class portable_binary_iarchive : { typedef boost::archive::basic_binary_iprimitive< portable_binary_iarchive, - std::istream::char_type, + std::istream::char_type, std::istream::traits_type > primitive_base_t; typedef boost::archive::detail::common_iarchive< @@ -165,26 +167,34 @@ class portable_binary_iarchive : this->primitive_base_t::load(t); } // intermediate level to support override of operators - // fot templates in the absence of partial function + // fot templates in the absence of partial function // template ordering - typedef boost::archive::detail::common_iarchive + typedef boost::archive::detail::common_iarchive detail_common_iarchive; template + +// breaking changes in boost >=1.59 +#if BOOST_VERSION >= 105900 + void load_override(T & t){ + this->detail_common_iarchive::load_override(t); + } + void load_override(boost::archive::class_name_type & t); + void load_override(boost::archive::class_id_optional_type & /* t */){} +#else void load_override(T & t, BOOST_PFTO int){ this->detail_common_iarchive::load_override(t, 0); } void load_override(boost::archive::class_name_type & t, int); - // binary files don't include the optional information - void load_override( - boost::archive::class_id_optional_type & /* t */, - int - ){} + // binary files don't include the optional information + void load_override(boost::archive::class_id_optional_type & /* t */, int){} +#endif + void init(unsigned int flags); public: portable_binary_iarchive(std::istream & is, unsigned flags = 0) : primitive_base_t( - * is.rdbuf(), + * is.rdbuf(), 0 != (flags & boost::archive::no_codecvt) ), archive_base_t(flags), @@ -195,13 +205,13 @@ class portable_binary_iarchive : portable_binary_iarchive( std::basic_streambuf< - std::istream::char_type, + std::istream::char_type, std::istream::traits_type - > & bsb, + > & bsb, unsigned int flags ) : primitive_base_t( - bsb, + bsb, 0 != (flags & boost::archive::no_codecvt) ), archive_base_t(flags), diff --git a/Schweizer-Messer/sm_boost/include/boost/portable_binary_oarchive.hpp b/Schweizer-Messer/sm_boost/include/boost/portable_binary_oarchive.hpp index ff5ac9d1d..aed96df80 100755 --- a/Schweizer-Messer/sm_boost/include/boost/portable_binary_oarchive.hpp +++ b/Schweizer-Messer/sm_boost/include/boost/portable_binary_oarchive.hpp @@ -14,7 +14,7 @@ /////////1/////////2/////////3/////////4/////////5/////////6/////////7/////////8 // portable_binary_oarchive.hpp -// (C) Copyright 2002 Robert Ramey - http://www.rrsd.com . +// (C) Copyright 2002 Robert Ramey - http://www.rrsd.com . // Use, modification and distribution is subject to the Boost Software // License, Version 1.0. (See accompanying file LICENSE_1_0.txt or copy at // http://www.boost.org/LICENSE_1_0.txt) @@ -32,16 +32,16 @@ namespace boost { namespace archive { - + /////////1/////////2/////////3/////////4/////////5/////////6/////////7/////////8 // exception to be thrown if integer read from archive doesn't fit // variable being loaded -class portable_binary_oarchive_exception : +class portable_binary_oarchive_exception : public virtual boost::archive::archive_exception { public: typedef enum { - invalid_flags + invalid_flags } exception_code; portable_binary_oarchive_exception(exception_code /* c */ ) {} @@ -61,14 +61,14 @@ class portable_binary_oarchive_exception : }; /////////1/////////2/////////3/////////4/////////5/////////6/////////7/////////8 -// "Portable" output binary archive. This is a variation of the native binary +// "Portable" output binary archive. This is a variation of the native binary // archive. it addresses integer size and endienness so that binary archives can // be passed across systems. Note:floating point types not addressed here class portable_binary_oarchive : public boost::archive::basic_binary_oprimitive< portable_binary_oarchive, - std::ostream::char_type, + std::ostream::char_type, std::ostream::traits_type >, public boost::archive::detail::common_oarchive< @@ -77,7 +77,7 @@ class portable_binary_oarchive : { typedef boost::archive::basic_binary_oprimitive< portable_binary_oarchive, - std::ostream::char_type, + std::ostream::char_type, std::ostream::traits_type > primitive_base_t; typedef boost::archive::detail::common_oarchive< @@ -133,9 +133,22 @@ class portable_binary_oarchive : // default processing - kick back to base class. Note the // extra stuff to get it passed borland compilers - typedef boost::archive::detail::common_oarchive + typedef boost::archive::detail::common_oarchive detail_common_oarchive; template +// breaking changes in boost >=1.59 +#if BOOST_VERSION >= 105900 + void save_override(T & t){ + this->detail_common_oarchive::save_override(t); + } + // explicitly convert to char * to avoid compile ambiguities + void save_override(const boost::archive::class_name_type & t){ + const std::string s(t); + * this << s; + } + // binary files don't include the optional information + void save_override(const boost::archive::class_id_optional_type & /* t */){} +#else void save_override(T & t, BOOST_PFTO int){ this->detail_common_oarchive::save_override(t, 0); } @@ -144,11 +157,9 @@ class portable_binary_oarchive : const std::string s(t); * this << s; } - // binary files don't include the optional information - void save_override( - const boost::archive::class_id_optional_type & /* t */, - int - ){} + // binary files don't include the optional information + void save_override(const boost::archive::class_id_optional_type & /* t */, int){} +#endif void init(unsigned int flags); public: @@ -165,13 +176,13 @@ class portable_binary_oarchive : portable_binary_oarchive( std::basic_streambuf< - std::ostream::char_type, + std::ostream::char_type, std::ostream::traits_type - > & bsb, + > & bsb, unsigned int flags ) : primitive_base_t( - bsb, + bsb, 0 != (flags & boost::archive::no_codecvt) ), archive_base_t(flags), diff --git a/Schweizer-Messer/sm_boost/src/portable_binary_iarchive.cpp b/Schweizer-Messer/sm_boost/src/portable_binary_iarchive.cpp index c763f4d97..107276873 100755 --- a/Schweizer-Messer/sm_boost/src/portable_binary_iarchive.cpp +++ b/Schweizer-Messer/sm_boost/src/portable_binary_iarchive.cpp @@ -1,7 +1,7 @@ /////////1/////////2/////////3/////////4/////////5/////////6/////////7/////////8 // portable_binary_iarchive.cpp -// (C) Copyright 2002 Robert Ramey - http://www.rrsd.com . +// (C) Copyright 2002 Robert Ramey - http://www.rrsd.com . // Use, modification and distribution is subject to the Boost Software // License, Version 1.0. (See accompanying file LICENSE_1_0.txt or copy at // http://www.boost.org/LICENSE_1_0.txt) @@ -19,7 +19,7 @@ namespace boost { namespace archive { -void +void portable_binary_iarchive::load_impl(boost::intmax_t & l, char maxsize){ char size; l = 0; @@ -50,18 +50,28 @@ portable_binary_iarchive::load_impl(boost::intmax_t & l, char maxsize){ if(m_flags & endian_big) #endif reverse_bytes(size, cptr); - + if(negative) l = -l; } void portable_binary_iarchive::load_override( - boost::archive::class_name_type & t, int + boost::archive::class_name_type & t +// breaking changes in boost >=1.59 +#if BOOST_VERSION >= 105900 +#else + , int +#endif ){ std::string cn; cn.reserve(BOOST_SERIALIZATION_MAX_KEY_SIZE); +// breaking changes in boost >=1.59 +#if BOOST_VERSION >= 105900 + load_override(cn); +#else load_override(cn, 0); +#endif if(cn.size() > (BOOST_SERIALIZATION_MAX_KEY_SIZE - 1)) boost::serialization::throw_exception( boost::archive::archive_exception( @@ -72,7 +82,7 @@ portable_binary_iarchive::load_override( t.t[cn.size()] = '\0'; } -void +void portable_binary_iarchive::init(unsigned int flags){ if(0 == (flags & boost::archive::no_header)){ // read signature in an archive version independent manner @@ -96,7 +106,7 @@ portable_binary_iarchive::init(unsigned int flags){ boost::archive::archive_exception::unsupported_version ) ); - + #if BOOST_WORKAROUND(__MWERKS__, BOOST_TESTED_AT(0x3205)) this->set_library_version(input_library_version); //#else @@ -127,7 +137,7 @@ namespace detail { template class basic_binary_iprimitive< portable_binary_iarchive, - std::istream::char_type, + std::istream::char_type, std::istream::traits_type > ; diff --git a/aslam_cv/aslam_cameras/include/aslam/cameras/implementation/CameraGeometry.hpp b/aslam_cv/aslam_cameras/include/aslam/cameras/implementation/CameraGeometry.hpp index 0a484514d..a2d4cff47 100644 --- a/aslam_cv/aslam_cameras/include/aslam/cameras/implementation/CameraGeometry.hpp +++ b/aslam_cv/aslam_cameras/include/aslam/cameras/implementation/CameraGeometry.hpp @@ -2,756 +2,764 @@ namespace aslam { -namespace cameras { - -/// \brief default constructor -template -CameraGeometry::CameraGeometry() { - -} - -/// \brief Construct from projection -template -CameraGeometry::CameraGeometry(const sm::PropertyTree & config) - : _projection(sm::PropertyTree(config, "projection")), - _shutter(sm::PropertyTree(config, "shutter")), - _mask(sm::PropertyTree(config, "mask")) { - -} - -/// \brief Construct from projection -template -CameraGeometry::CameraGeometry(const projection_t & projection) - : _projection(projection) { - -} - -/// \brief Construct from projection and shutter -template -CameraGeometry::CameraGeometry(const projection_t & projection, - const shutter_t & shutter) - : _projection(projection), - _shutter(shutter) { - -} - -/// \brief Construct from projection, shutter, and mask -template -CameraGeometry::CameraGeometry(const projection_t & projection, - const shutter_t & shutter, - const mask_t & mask) - : _projection(projection), - _shutter(shutter), - _mask(mask) { - -} - -/// \brief simple destructor -template -CameraGeometry::~CameraGeometry() { - -} - -template -bool CameraGeometry::vsEuclideanToKeypoint( - const Eigen::Vector3d & p, Eigen::VectorXd & outKeypoint) const { - bool valid = _projection.euclideanToKeypoint(p, outKeypoint); - return valid && _mask.isValid(outKeypoint); -} - -template -bool CameraGeometry::vsEuclideanToKeypoint( - const Eigen::Vector3d & p, Eigen::VectorXd & outKeypoint, - Eigen::MatrixXd & outJacobian) const { - bool valid = _projection.euclideanToKeypoint(p, outKeypoint, outJacobian); - return valid && _mask.isValid(outKeypoint); -} - -template -bool CameraGeometry::vsHomogeneousToKeypoint( - const Eigen::Vector4d & ph, Eigen::VectorXd & outKeypoint) const { - bool valid = _projection.homogeneousToKeypoint(ph, outKeypoint); - return valid && _mask.isValid(outKeypoint); -} - -template -bool CameraGeometry::vsHomogeneousToKeypoint( - const Eigen::Vector4d & ph, Eigen::VectorXd & outKeypoint, - Eigen::MatrixXd & outJacobian) const { - bool valid = _projection.homogeneousToKeypoint(ph, outKeypoint, outJacobian); - return valid && _mask.isValid(outKeypoint); -} - -template -bool CameraGeometry::vsKeypointToEuclidean( - const Eigen::VectorXd & keypoint, Eigen::Vector3d & outPoint) const { - return _projection.keypointToEuclidean(keypoint, outPoint) - && _mask.isValid(keypoint); -} - -template -bool CameraGeometry::vsKeypointToEuclidean( - const Eigen::VectorXd & keypoint, Eigen::VectorXd & outPoint, - Eigen::MatrixXd & outJacobian) const { - return _projection.keypointToEuclidean(keypoint, outPoint, outJacobian) - && _mask.isValid(keypoint); -} - -template -bool CameraGeometry::vsKeypointToHomogeneous( - Eigen::VectorXd const & keypoint, Eigen::VectorXd & outPoint) const { - return _projection.keypointToHomogeneous(keypoint, outPoint) - && _mask.isValid(keypoint); -} - -template -bool CameraGeometry::vsKeypointToHomogeneous( - Eigen::VectorXd const & keypoint, Eigen::VectorXd & outPoint, - Eigen::MatrixXd & outJacobian) const { - return _projection.keypointToHomogeneous(keypoint, outPoint, outJacobian) - && _mask.isValid(keypoint); -} - -template -bool CameraGeometry::vsIsValid( - const Eigen::VectorXd & keypoint) const { - return isValid(keypoint); -} - -template -bool CameraGeometry::vsIsEuclideanVisible( - const Eigen::Vector3d & p) const { - return _projection.isEuclideanVisible(p); -} - -template -bool CameraGeometry::vsIsHomogeneousVisible( - const Eigen::Vector4d & ph) const { - return _projection.isHomogeneousVisible(ph); -} - -// The amount of time elapsed between the start of the image and the -// keypoint. For a global shutter camera, this can return Duration(0). -template -Duration CameraGeometry::vsTemporalOffset( - const Eigen::VectorXd & keypoint) const { - return _shutter.temporalOffset(keypoint); -} - -/// \brief get the camera id. -template -CameraId CameraGeometry::id() const { - return _id; -} - -/// \brief set the camera id. -template -void CameraGeometry::setId(CameraId id) { - _id = id; -} - -/// \brief the length of a keypoint -template -size_t CameraGeometry::keypointDimension() const { - return P::KeypointDimension; -} - -// \brief creates a random valid keypoint. -template -Eigen::VectorXd CameraGeometry::createRandomKeypoint() const { - return _projection.createRandomKeypoint(); -} - -// \brief creates a random visible point. Negative depth means random between 0 and 100 meters. -template -Eigen::Vector3d CameraGeometry::createRandomVisiblePoint( - double depth) const { - return _projection.createRandomVisiblePoint(depth); -} - -template -template -bool CameraGeometry::euclideanToKeypoint( - const Eigen::MatrixBase & p, - const Eigen::MatrixBase & outKeypoint) const { - bool valid = _projection.euclideanToKeypoint(p, outKeypoint); - return valid && _mask.isValid(outKeypoint); -} - -template -template -bool CameraGeometry::euclideanToKeypoint( - const Eigen::MatrixBase & p, - const Eigen::MatrixBase & outKeypoint, - const Eigen::MatrixBase & outJp) const { - bool valid = _projection.euclideanToKeypoint(p, outKeypoint, outJp); - return valid && _mask.isValid(outKeypoint); -} - -template -template -bool CameraGeometry::euclideanToKeypointFiniteDifference( - const Eigen::MatrixBase & p, - const Eigen::MatrixBase & outKeypoint, - const Eigen::MatrixBase & outJp) const { - - DERIVED_JP & Jp = const_cast&>(outJp).derived(); - ASLAM_CAMERAS_ESTIMATE_JACOBIAN(this, euclideanToKeypoint, p, 1e-5, Jp); - - bool valid = _projection.euclideanToKeypoint(p, outKeypoint); - return valid && _mask.isValid(outKeypoint); -} - -// Project the point and get the associated uncertainty of the projection. -template -template -bool CameraGeometry::homogeneousToKeypoint( - const sm::kinematics::UncertainHomogeneousPoint & p, - const Eigen::MatrixBase & outKeypoint, - covariance_t & outProjectionUncertainty) const { - jacobian_homogeneous_t J; - bool r = - static_cast *>(this) - ->homogeneousToKeypoint(p.toHomogeneous(), outKeypoint, J); - - outProjectionUncertainty = J * p.U4() * J.transpose(); - return r; -} - -template -template -bool CameraGeometry::homogeneousToKeypoint( - const sm::kinematics::HomogeneousPoint & p, - const Eigen::MatrixBase & outKeypoint) const { - return static_cast *>(this) - ->homogeneousToKeypoint(p.toHomogeneous(), outKeypoint); -} - -template -template -bool CameraGeometry::homogeneousToKeypoint( - const Eigen::MatrixBase & p, - const Eigen::MatrixBase & outKeypoint) const { - bool valid = _projection.homogeneousToKeypoint(p, outKeypoint); - return valid && _mask.isValid(outKeypoint); -} - -template -template -bool CameraGeometry::homogeneousToKeypoint( - const Eigen::MatrixBase & p, - const Eigen::MatrixBase & outKeypoint, - const Eigen::MatrixBase & outJp) const { - bool valid = _projection.homogeneousToKeypoint(p, outKeypoint, outJp); - return valid && _mask.isValid(outKeypoint); -} - -template -template -bool CameraGeometry::homogeneousToKeypointFiniteDifference( - const Eigen::MatrixBase & p, - const Eigen::MatrixBase & outKeypoint, - const Eigen::MatrixBase & outJp) const { - - DERIVED_JP & Jp = const_cast&>(outJp).derived(); - ASLAM_CAMERAS_ESTIMATE_JACOBIAN(this, homogeneousToKeypoint, p, 1e-5, Jp); - - return _mask.isValid(_projection.homogeneousToKeypoint(p, outKeypoint)); -} - -template -template -bool CameraGeometry::keypointToEuclidean( - const Eigen::MatrixBase & keypoint, - const Eigen::MatrixBase & outPoint) const { - // I'm putting the mask second in this case to make sure the keypointToEuclidean function is called. - return _projection.keypointToEuclidean(keypoint, outPoint) - && _mask.isValid(keypoint); -} - -template -template -bool CameraGeometry::keypointToEuclidean( - const Eigen::MatrixBase & keypoint, - const Eigen::MatrixBase & outPoint, - const Eigen::MatrixBase & outJk) const { - // I'm putting the mask second in this case to make sure the keypointToEuclidean function is called. - return _projection.keypointToEuclidean(keypoint, outPoint, outJk) - && _mask.isValid(keypoint); -} - -template -template -bool CameraGeometry::keypointToEuclideanFiniteDifference( - const Eigen::MatrixBase & keypoint, - const Eigen::MatrixBase & outPoint, - const Eigen::MatrixBase & outJk) const { - DERIVED_JK & Jk = const_cast&>(outJk).derived(); - ASLAM_CAMERAS_ESTIMATE_JACOBIAN(this, keypointToEuclidean, keypoint, 1e-5, - Jk); - - // I'm putting the mask second in this case to make sure the keypointToEuclidean function is called. - return _projection.keypointToEuclidean(keypoint, outPoint) - && _mask.isValid(keypoint); -} - -template -template -bool CameraGeometry::keypointToHomogeneous( - const Eigen::MatrixBase & keypoint, - const Eigen::MatrixBase & outPoint) const { - // I'm putting the mask second in this case to make sure the keypointToEuclidean function is called. - return _projection.keypointToHomogeneous(keypoint, outPoint) - && _mask.isValid(keypoint); -} - -template -template -bool CameraGeometry::keypointToHomogeneous( - const Eigen::MatrixBase & keypoint, - const Eigen::MatrixBase & outPoint, - const Eigen::MatrixBase & outJk) const { - // I'm putting the mask second in this case to make sure the keypointToEuclidean function is called. - return _projection.keypointToHomogeneous(keypoint, outPoint, outJk) - && _mask.isValid(keypoint); -} - -template -template -bool CameraGeometry::keypointToHomogeneousFiniteDifference( - const Eigen::MatrixBase & keypoint, - const Eigen::MatrixBase & outPoint, - const Eigen::MatrixBase & outJk) const { - DERIVED_JK & Jk = const_cast&>(outJk).derived(); - ASLAM_CAMERAS_ESTIMATE_JACOBIAN(this, keypointToHomogeneous, keypoint, 1e-5, - Jk); - - // I'm putting the mask second in this case to make sure the keypointToEuclidean function is called. - return _projection.keypointToHomogeneous(keypoint, outPoint) - && _mask.isValid(keypoint); -} - -template -template -void CameraGeometry::euclideanToKeypointIntrinsicsJacobian( - const Eigen::MatrixBase & p, - const Eigen::MatrixBase & outJi) const { - _projection.euclideanToKeypointIntrinsicsJacobian(p, outJi); -} - -template -template -void CameraGeometry::euclideanToKeypointDistortionJacobian( - const Eigen::MatrixBase & p, - const Eigen::MatrixBase & outJd) const { - _projection.euclideanToKeypointDistortionJacobian(p, outJd); -} - -template -template -void CameraGeometry::homogeneousToKeypointIntrinsicsJacobian( - const Eigen::MatrixBase & p, - const Eigen::MatrixBase & outJi) const { - _projection.homogeneousToKeypointIntrinsicsJacobian(p, outJi); -} - -template -template -void CameraGeometry::homogeneousToKeypointDistortionJacobian( - const Eigen::MatrixBase & p, - const Eigen::MatrixBase & outJd) const { - return _projection.homogeneousToKeypointDistortionJacobian(p, outJd); -} - -template -template -void CameraGeometry::euclideanToKeypointIntrinsicsJacobianFiniteDifference( - const Eigen::MatrixBase & p, - const Eigen::MatrixBase & outJi) const { - DERIVED_JI & Ji = const_cast&>(outJi).derived(); - projection_t proj = _projection; - ASLAM_CAMERAS_ESTIMATE_INTRINSIC_JACOBIAN(euclideanToKeypoint, proj, proj, p, - 1e-5, Ji); - -} - -template -template -void CameraGeometry::homogeneousToKeypointIntrinsicsJacobianFiniteDifference( - const Eigen::MatrixBase & p, - const Eigen::MatrixBase & outJi) const { - DERIVED_JI & Ji = const_cast&>(outJi).derived(); - projection_t proj = _projection; - ASLAM_CAMERAS_ESTIMATE_INTRINSIC_JACOBIAN(homogeneousToKeypoint, proj, proj, - p, 1e-5, Ji); - -} - -template -template -void CameraGeometry::euclideanToKeypointDistortionJacobianFiniteDifference( - const Eigen::MatrixBase & p, - const Eigen::MatrixBase & outJd) const { - DERIVED_JD & Jd = const_cast&>(outJd).derived(); - projection_t proj = _projection; - ASLAM_CAMERAS_ESTIMATE_INTRINSIC_JACOBIAN(euclideanToKeypoint, proj, - proj.distortion(), p, 1e-5, Jd); -} - -template -template -void CameraGeometry::homogeneousToKeypointDistortionJacobianFiniteDifference( - const Eigen::MatrixBase & p, - const Eigen::MatrixBase & outJd) const { - DERIVED_JD & Jd = const_cast&>(outJd).derived(); - projection_t proj = _projection; - ASLAM_CAMERAS_ESTIMATE_INTRINSIC_JACOBIAN(homogeneousToKeypoint, proj, - proj.distortion(), p, 1e-5, Jd); -} - -////////////////////////////////////////////////////////////// -// SHUTTER SUPPORT -////////////////////////////////////////////////////////////// - -// The amount of time elapsed between the start of the image and the -// keypoint. For a global shutter camera, this can return CameraGeometry::Duration(0). -template -template -Duration CameraGeometry::temporalOffset( - const Eigen::MatrixBase & keypoint) const { - return _shutter.temporalOffset(keypoint); -} - -template -Duration CameraGeometry::temporalOffset( - const Eigen::VectorXd& keypoint) const { - return _shutter.temporalOffset(keypoint); -} - -////////////////////////////////////////////////////////////// -// VALIDITY TESTING -////////////////////////////////////////////////////////////// - -template -template -bool CameraGeometry::isValid( - const Eigen::MatrixBase & keypoint) const { - return _mask.isValid(keypoint) && _projection.isValid(keypoint); -} - -template -template -bool CameraGeometry::isEuclideanVisible( - const Eigen::MatrixBase & p) const { - keypoint_t k; - bool success = _projection.euclideanToKeypoint(p, k); - - return success && _mask.isValid(k); -} - -template -template -bool CameraGeometry::isHomogeneousVisible( - const Eigen::MatrixBase & ph) const { - keypoint_t k; - bool success = _projection.homogeneousToKeypoint(ph, k); - - return success && _mask.isValid(k); - -} - -template -bool CameraGeometry::isProjectionInvertible() const { - return _projection.isProjectionInvertible(); -} - -template -bool CameraGeometry::isBinaryEqual( - const CameraGeometry & rhs) const { - return SM_CHECKMEMBERSSAME(rhs, _id) && SM_CHECKMEMBERSSAME(rhs, _projection) - && SM_CHECKMEMBERSSAME(rhs, _shutter) && SM_CHECKMEMBERSSAME(rhs, _mask); -} - -template -CameraGeometry CameraGeometry::getTestGeometry() { - CameraGeometry camera(projection_t::getTestProjection(), - shutter_t::getTestShutter(), mask_t()); - camera.setId(CameraId(rand())); - return camera; -} - -/// \brief initialize the intrinsics based on list of views of a gridded calibration target - -template -bool CameraGeometry::initializeIntrinsics(const std::vector &observations) { - return _projection.initializeIntrinsics(observations); -} - -/// \brief estimate the transformation of the camera with respect to the calibration target -template -bool CameraGeometry::estimateTransformation( - const GridCalibrationTargetObservation & obs, - sm::kinematics::Transformation & out_T_t_c) const { - return _projection.estimateTransformation(obs, out_T_t_c); -} - -template -boost::shared_ptr::frame_t> CameraGeometry::createUninitializedFrame() const { - - boost::shared_ptr < frame_t > frame(new frame_t); - return frame; - -} - -template -boost::shared_ptr CameraGeometry::createUninitializedFrameBase() const { - - boost::shared_ptr < FrameBase > frame(new frame_t); - return frame; - -} - -template -void CameraGeometry::print(std::ostream & out) { - std::cout << "Printing!\n"; - Eigen::MatrixXd P; - _projection.getParameters(P); - out << "Projection: " << P.transpose() << std::endl; - _projection.distortion().getParameters(P); - if (P.rows() > 0 && P.cols() > 0) { - out << "Distortion: " << P.transpose() << std::endl; - } -} - -/// \brief the minimal dimensions of the projection parameters -template -int CameraGeometry::minimalDimensionsProjection() const { - return _projection.minimalDimensions(); -} - -/// \brief the minimal dimensions of the distortion parameters -template -int CameraGeometry::minimalDimensionsDistortion() const { - return _projection.distortion().minimalDimensions(); -} - -template -int CameraGeometry::minimalDimensionsShutter() const { - return _shutter.minimalDimensions(); -} - -/// \brief update the intrinsics -template -void CameraGeometry::update(const double * v, bool estimateProjection, - bool estimateDistortion, - bool estimateShutter) { - if (estimateProjection) { - _projection.update(v); - v += _projection.minimalDimensions(); - } - - if (estimateDistortion) { - _projection.distortion().update(v); - v += _projection.distortion().minimalDimensions(); - } - - if (estimateShutter) { - _shutter.update(v); - } -} - -/// \brief Get the total number of dimensions of the intrinsic parameters -template -int CameraGeometry::minimalDimensions(bool estimateProjection, - bool estimateDistortion, - bool estimateShutter) const { - int dim = 0; - if (estimateProjection) { - dim += _projection.minimalDimensions(); - } - - if (estimateDistortion) { - dim += _projection.distortion().minimalDimensions(); - } - - if (estimateShutter) { - dim += _shutter.minimalDimensions(); - } - return dim; -} - -/// \brief get the intrinsic parameters. -template -void CameraGeometry::getParameters(Eigen::MatrixXd & params, - bool estimateProjection, - bool estimateDistortion, - bool estimateShutter) const { - Eigen::MatrixXd Pp, Pd, Ps; - int rows = 0, cols = 0; - if (estimateProjection) { - _projection.getParameters(Pp); - rows += Pp.rows(); - cols = std::max(cols, (int) Pp.cols()); - } - - if (estimateDistortion) { - _projection.distortion().getParameters(Pd); - rows += Pd.rows(); - cols = std::max(cols, (int) Pd.cols()); - } - - if (estimateShutter) { - _shutter.getParameters(Ps); - rows += Ps.rows(); - cols = std::max(cols, (int) Ps.cols()); - } - - params = Eigen::MatrixXd::Zero(rows, cols); - int row = 0; - - if (estimateProjection) { - params.block(row, 0, Pp.rows(), Pp.cols()) = Pp; - row += Pp.rows(); - } - - if (estimateDistortion) { - params.block(row, 0, Pd.rows(), Pd.cols()) = Pd; - row += Pd.rows(); - } - - if (estimateShutter) { - params.block(row, 0, Ps.rows(), Ps.cols()) = Ps; - row += Ps.rows(); - } - -} - -/// \brief set the intrinsic parameters. -template -void CameraGeometry::setParameters(const Eigen::MatrixXd & params, - bool estimateProjection, - bool estimateDistortion, - bool estimateShutter) { - // Ugh...I wish I had used a vector, not a matrix. - int row = 0; - if (estimateProjection) { - Eigen::Vector2i sz = _projection.parameterSize(); - _projection.setParameters(params.block(row, 0, sz[0], sz[1])); - row += sz[0]; - } - - if (estimateDistortion) { - Eigen::Vector2i sz = _projection.distortion().parameterSize(); - _projection.distortion().setParameters(params.block(row, 0, sz[0], sz[1])); - row += sz[0]; - } - - if (estimateShutter) { - Eigen::Vector2i sz = _shutter.parameterSize(); - _shutter.setParameters(params.block(row, 0, sz[0], sz[1])); - row += sz[0]; - } - -} - -/// \brief return the Jacobian of the projection with respect to the intrinsics. -template -void CameraGeometry::euclideanToKeypointIntrinsicsJacobian( - const Eigen::Vector3d & p, Eigen::MatrixXd & outJi, bool estimateProjection, - bool estimateDistortion, bool estimateShutter) const { - // Here I have to stack the answers. The resulting matrix should be - // KeypointDimension x minimalDimensions() - outJi = Eigen::MatrixXd::Zero( - KeypointDimension, - minimalDimensions(estimateProjection, estimateDistortion, - estimateShutter)); - - int col = 0; - if (estimateProjection) { - Eigen::MatrixXd J; - _projection.euclideanToKeypointIntrinsicsJacobian(p, J); - outJi.block(col, 0, KeypointDimension, _projection.minimalDimensions()) = J; - col += _projection.minimalDimensions(); - } - - if (estimateDistortion) { - Eigen::MatrixXd J; - _projection.euclideanToKeypointDistortionJacobian(p, J); - outJi.block(col, 0, KeypointDimension, - _projection.distortion().minimalDimensions()) = J; - col += _projection.distortion().minimalDimensions(); - } - - if (estimateShutter) { - // Uh...nothing. The shutter acts on the keypoint time. - // At least for any example I can think of. - } - -} - -/// \brief return the Jacobian of the projection with respect to the intrinsics. -template -void CameraGeometry::homogeneousToKeypointIntrinsicsJacobian( - const Eigen::Vector4d & p, Eigen::MatrixXd & outJi, bool estimateProjection, - bool estimateDistortion, bool estimateShutter) const { - // Here I have to stack the answers. The resulting matrix should be - // KeypointDimension x minimalDimensions() - outJi = Eigen::MatrixXd::Zero( - KeypointDimension, - minimalDimensions(estimateProjection, estimateDistortion, - estimateShutter)); - - int col = 0; - if (estimateProjection) { - Eigen::MatrixXd J; - _projection.homogeneousToKeypointIntrinsicsJacobian(p, J); - outJi.block(0, col, KeypointDimension, _projection.minimalDimensions()) = J; - col += _projection.minimalDimensions(); - } - - if (estimateDistortion) { - Eigen::MatrixXd J; - _projection.homogeneousToKeypointDistortionJacobian(p, J); - outJi.block(0, col, KeypointDimension, - _projection.distortion().minimalDimensions()) = J; - col += _projection.distortion().minimalDimensions(); - } - - if (estimateShutter) { - // Uh...nothing. The shutter acts on the keypoint time. - // At least for any example I can think of. - } - -} - -/// \brief return the temporal offset with respect to the intrinsics. -template -void CameraGeometry::temporalOffsetIntrinsicsJacobian( - const Eigen::VectorXd & keypoint, Eigen::MatrixXd & outJi, - bool estimateProjection, bool estimateDistortion, - bool estimateShutter) const { - // Here I have to stack the answers. The resulting matrix should be - // 1 x totalparameterSize - outJi = Eigen::MatrixXd::Zero( - 1, - minimalDimensions(estimateProjection, estimateDistortion, - estimateShutter)); - - int col = 0; - if (estimateProjection) { - col += _projection.minimalDimensions(); - } - - if (estimateDistortion) { - col += _projection.distortion().minimalDimensions(); - } - - if (estimateShutter) { - _shutter.temporalOffsetIntrinsicsJacobian( - keypoint, outJi.block(0, col, 1, _shutter.minimalDimensions())); - } -} - -} // namespace cameras + namespace cameras { + + /// \brief default constructor + template + CameraGeometry::CameraGeometry() { + + } + + /// \brief Construct from projection + template + CameraGeometry::CameraGeometry(const sm::PropertyTree & config) + : _projection(sm::PropertyTree(config, "projection")), + _shutter(sm::PropertyTree(config, "shutter")), + _mask(sm::PropertyTree(config, "mask")) { + + } + + /// \brief Construct from projection + template + CameraGeometry::CameraGeometry(const projection_t & projection) + : _projection(projection) { + + } + + /// \brief Construct from projection and shutter + template + CameraGeometry::CameraGeometry(const projection_t & projection, + const shutter_t & shutter) + : _projection(projection), + _shutter(shutter) { + + } + + /// \brief Construct from projection, shutter, and mask + template + CameraGeometry::CameraGeometry(const projection_t & projection, + const shutter_t & shutter, + const mask_t & mask) + : _projection(projection), + _shutter(shutter), + _mask(mask) { + + } + + /// \brief simple destructor + template + CameraGeometry::~CameraGeometry() { + + } + + template + bool CameraGeometry::vsEuclideanToKeypoint( + const Eigen::Vector3d & p, Eigen::VectorXd & outKeypoint) const { + bool valid = _projection.euclideanToKeypoint(p, outKeypoint); + return valid && _mask.isValid(outKeypoint); + } + + template + bool CameraGeometry::vsEuclideanToKeypoint( + const Eigen::Vector3d & p, Eigen::VectorXd & outKeypoint, + Eigen::MatrixXd & outJacobian) const { + bool valid = _projection.euclideanToKeypoint(p, outKeypoint, outJacobian); + return valid && _mask.isValid(outKeypoint); + } + + template + bool CameraGeometry::vsHomogeneousToKeypoint( + const Eigen::Vector4d & ph, Eigen::VectorXd & outKeypoint) const { + bool valid = _projection.homogeneousToKeypoint(ph, outKeypoint); + return valid && _mask.isValid(outKeypoint); + } + + template + bool CameraGeometry::vsHomogeneousToKeypoint( + const Eigen::Vector4d & ph, Eigen::VectorXd & outKeypoint, + Eigen::MatrixXd & outJacobian) const { + bool valid = _projection.homogeneousToKeypoint(ph, outKeypoint, outJacobian); + return valid && _mask.isValid(outKeypoint); + } + + template + bool CameraGeometry::vsKeypointToEuclidean( + const Eigen::VectorXd & keypoint, Eigen::Vector3d & outPoint) const { + return _projection.keypointToEuclidean(keypoint, outPoint) + && _mask.isValid(keypoint); + } + + template + bool CameraGeometry::vsKeypointToEuclidean( + const Eigen::VectorXd & keypoint, Eigen::VectorXd & outPoint, + Eigen::MatrixXd & outJacobian) const { + return _projection.keypointToEuclidean(keypoint, outPoint, outJacobian) + && _mask.isValid(keypoint); + } + + template + bool CameraGeometry::vsKeypointToHomogeneous( + Eigen::VectorXd const & keypoint, Eigen::VectorXd & outPoint) const { + return _projection.keypointToHomogeneous(keypoint, outPoint) + && _mask.isValid(keypoint); + } + + template + bool CameraGeometry::vsKeypointToHomogeneous( + Eigen::VectorXd const & keypoint, Eigen::VectorXd & outPoint, + Eigen::MatrixXd & outJacobian) const { + return _projection.keypointToHomogeneous(keypoint, outPoint, outJacobian) + && _mask.isValid(keypoint); + } + + template + bool CameraGeometry::vsIsValid( + const Eigen::VectorXd & keypoint) const { + return isValid(keypoint); + } + + template + bool CameraGeometry::vsIsEuclideanVisible( + const Eigen::Vector3d & p) const { + return _projection.isEuclideanVisible(p); + } + + template + bool CameraGeometry::vsIsHomogeneousVisible( + const Eigen::Vector4d & ph) const { + return _projection.isHomogeneousVisible(ph); + } + + // The amount of time elapsed between the start of the image and the + // keypoint. For a global shutter camera, this can return Duration(0). + template + Duration CameraGeometry::vsTemporalOffset( + const Eigen::VectorXd & keypoint) const { + return _shutter.temporalOffset(keypoint); + } + + /// \brief get the camera id. + template + CameraId CameraGeometry::id() const { + return _id; + } + + /// \brief set the camera id. + template + void CameraGeometry::setId(CameraId id) { + _id = id; + } + + /// \brief the length of a keypoint + template + size_t CameraGeometry::keypointDimension() const { + return P::KeypointDimension; + } + + // \brief creates a random valid keypoint. + template + Eigen::VectorXd CameraGeometry::createRandomKeypoint() const { + return _projection.createRandomKeypoint(); + } + + // \brief creates a random visible point. Negative depth means random between 0 and 100 meters. + template + Eigen::Vector3d CameraGeometry::createRandomVisiblePoint( + double depth) const { + return _projection.createRandomVisiblePoint(depth); + } + + template + template + bool CameraGeometry::euclideanToKeypoint( + const Eigen::MatrixBase & p, + const Eigen::MatrixBase & outKeypoint) const { + bool valid = _projection.euclideanToKeypoint(p, outKeypoint); + return valid && _mask.isValid(outKeypoint); + } + + template + template + bool CameraGeometry::euclideanToKeypoint( + const Eigen::MatrixBase & p, + const Eigen::MatrixBase & outKeypoint, + const Eigen::MatrixBase & outJp) const { + bool valid = _projection.euclideanToKeypoint(p, outKeypoint, outJp); + return valid && _mask.isValid(outKeypoint); + } + + template + template + bool CameraGeometry::euclideanToKeypointFiniteDifference( + const Eigen::MatrixBase & p, + const Eigen::MatrixBase & outKeypoint, + const Eigen::MatrixBase & outJp) const { + + DERIVED_JP & Jp = const_cast&>(outJp).derived(); + ASLAM_CAMERAS_ESTIMATE_JACOBIAN(this, euclideanToKeypoint, p, 1e-5, Jp); + + bool valid = _projection.euclideanToKeypoint(p, outKeypoint); + return valid && _mask.isValid(outKeypoint); + } + + // Project the point and get the associated uncertainty of the projection. + template + template + bool CameraGeometry::homogeneousToKeypoint( + const sm::kinematics::UncertainHomogeneousPoint & p, + const Eigen::MatrixBase & outKeypoint, + covariance_t & outProjectionUncertainty) const { + jacobian_homogeneous_t J; + bool r = + static_cast *>(this) + ->homogeneousToKeypoint(p.toHomogeneous(), outKeypoint, J); + + outProjectionUncertainty = J * p.U4() * J.transpose(); + return r; + } + + template + template + bool CameraGeometry::homogeneousToKeypoint( + const sm::kinematics::HomogeneousPoint & p, + const Eigen::MatrixBase & outKeypoint) const { + return static_cast *>(this) + ->homogeneousToKeypoint(p.toHomogeneous(), outKeypoint); + } + + template + template + bool CameraGeometry::homogeneousToKeypoint( + const Eigen::MatrixBase & p, + const Eigen::MatrixBase & outKeypoint) const { + bool valid = _projection.homogeneousToKeypoint(p, outKeypoint); + return valid && _mask.isValid(outKeypoint); + } + + template + template + bool CameraGeometry::homogeneousToKeypoint( + const Eigen::MatrixBase & p, + const Eigen::MatrixBase & outKeypoint, + const Eigen::MatrixBase & outJp) const { + bool valid = _projection.homogeneousToKeypoint(p, outKeypoint, outJp); + return valid && _mask.isValid(outKeypoint); + } + + template + template + bool CameraGeometry::homogeneousToKeypointFiniteDifference( + const Eigen::MatrixBase & p, + const Eigen::MatrixBase & outKeypoint, + const Eigen::MatrixBase & outJp) const { + + DERIVED_JP & Jp = const_cast&>(outJp).derived(); + ASLAM_CAMERAS_ESTIMATE_JACOBIAN(this, homogeneousToKeypoint, p, 1e-5, Jp); + + return _mask.isValid(_projection.homogeneousToKeypoint(p, outKeypoint)); + } + + template + template + bool CameraGeometry::keypointToEuclidean( + const Eigen::MatrixBase & keypoint, + const Eigen::MatrixBase & outPoint) const { + // I'm putting the mask second in this case to make sure the keypointToEuclidean function is called. + return _projection.keypointToEuclidean(keypoint, outPoint) + && _mask.isValid(keypoint); + } + + template + template + bool CameraGeometry::keypointToEuclidean( + const Eigen::MatrixBase & keypoint, + const Eigen::MatrixBase & outPoint, + const Eigen::MatrixBase & outJk) const { + // I'm putting the mask second in this case to make sure the keypointToEuclidean function is called. + return _projection.keypointToEuclidean(keypoint, outPoint, outJk) + && _mask.isValid(keypoint); + } + + template + template + bool CameraGeometry::keypointToEuclideanFiniteDifference( + const Eigen::MatrixBase & keypoint, + const Eigen::MatrixBase & outPoint, + const Eigen::MatrixBase & outJk) const { + DERIVED_JK & Jk = const_cast&>(outJk).derived(); + ASLAM_CAMERAS_ESTIMATE_JACOBIAN(this, keypointToEuclidean, keypoint, 1e-5, + Jk); + + // I'm putting the mask second in this case to make sure the keypointToEuclidean function is called. + return _projection.keypointToEuclidean(keypoint, outPoint) + && _mask.isValid(keypoint); + } + + template + template + bool CameraGeometry::keypointToHomogeneous( + const Eigen::MatrixBase & keypoint, + const Eigen::MatrixBase & outPoint) const { + // I'm putting the mask second in this case to make sure the keypointToEuclidean function is called. + return _projection.keypointToHomogeneous(keypoint, outPoint) + && _mask.isValid(keypoint); + } + + template + template + bool CameraGeometry::keypointToHomogeneous( + const Eigen::MatrixBase & keypoint, + const Eigen::MatrixBase & outPoint, + const Eigen::MatrixBase & outJk) const { + // I'm putting the mask second in this case to make sure the keypointToEuclidean function is called. + return _projection.keypointToHomogeneous(keypoint, outPoint, outJk) + && _mask.isValid(keypoint); + } + + template + template + bool CameraGeometry::keypointToHomogeneousFiniteDifference( + const Eigen::MatrixBase & keypoint, + const Eigen::MatrixBase & outPoint, + const Eigen::MatrixBase & outJk) const { + DERIVED_JK & Jk = const_cast&>(outJk).derived(); + ASLAM_CAMERAS_ESTIMATE_JACOBIAN(this, keypointToHomogeneous, keypoint, 1e-5, + Jk); + + // I'm putting the mask second in this case to make sure the keypointToEuclidean function is called. + return _projection.keypointToHomogeneous(keypoint, outPoint) + && _mask.isValid(keypoint); + } + + template + template + void CameraGeometry::euclideanToKeypointIntrinsicsJacobian( + const Eigen::MatrixBase & p, + const Eigen::MatrixBase & outJi) const { + _projection.euclideanToKeypointIntrinsicsJacobian(p, outJi); + } + + template + template + void CameraGeometry::euclideanToKeypointDistortionJacobian( + const Eigen::MatrixBase & p, + const Eigen::MatrixBase & outJd) const { + _projection.euclideanToKeypointDistortionJacobian(p, outJd); + } + + template + template + void CameraGeometry::homogeneousToKeypointIntrinsicsJacobian( + const Eigen::MatrixBase & p, + const Eigen::MatrixBase & outJi) const { + _projection.homogeneousToKeypointIntrinsicsJacobian(p, outJi); + } + + template + template + void CameraGeometry::homogeneousToKeypointDistortionJacobian( + const Eigen::MatrixBase & p, + const Eigen::MatrixBase & outJd) const { + return _projection.homogeneousToKeypointDistortionJacobian(p, outJd); + } + + // template + // template + // void CameraGeometry::homogeneousToKeypointShutterJacobian( + // const Eigen::MatrixBase & p, + // const Eigen::MatrixBase & outJd) const { + // return _shutter.homogeneousToKeypointShutterJacobian(p, outJd); + // } + + template + template + void CameraGeometry::euclideanToKeypointIntrinsicsJacobianFiniteDifference( + const Eigen::MatrixBase & p, + const Eigen::MatrixBase & outJi) const { + DERIVED_JI & Ji = const_cast&>(outJi).derived(); + projection_t proj = _projection; + ASLAM_CAMERAS_ESTIMATE_INTRINSIC_JACOBIAN(euclideanToKeypoint, proj, proj, p, + 1e-5, Ji); + + } + + template + template + void CameraGeometry::homogeneousToKeypointIntrinsicsJacobianFiniteDifference( + const Eigen::MatrixBase & p, + const Eigen::MatrixBase & outJi) const { + DERIVED_JI & Ji = const_cast&>(outJi).derived(); + projection_t proj = _projection; + ASLAM_CAMERAS_ESTIMATE_INTRINSIC_JACOBIAN(homogeneousToKeypoint, proj, proj, + p, 1e-5, Ji); + + } + + template + template + void CameraGeometry::euclideanToKeypointDistortionJacobianFiniteDifference( + const Eigen::MatrixBase & p, + const Eigen::MatrixBase & outJd) const { + DERIVED_JD & Jd = const_cast&>(outJd).derived(); + projection_t proj = _projection; + ASLAM_CAMERAS_ESTIMATE_INTRINSIC_JACOBIAN(euclideanToKeypoint, proj, + proj.distortion(), p, 1e-5, Jd); + } + + template + template + void CameraGeometry::homogeneousToKeypointDistortionJacobianFiniteDifference( + const Eigen::MatrixBase & p, + const Eigen::MatrixBase & outJd) const { + DERIVED_JD & Jd = const_cast&>(outJd).derived(); + projection_t proj = _projection; + ASLAM_CAMERAS_ESTIMATE_INTRINSIC_JACOBIAN(homogeneousToKeypoint, proj, + proj.distortion(), p, 1e-5, Jd); + } + + ////////////////////////////////////////////////////////////// + // SHUTTER SUPPORT + ////////////////////////////////////////////////////////////// + + // The amount of time elapsed between the start of the image and the + // keypoint. For a global shutter camera, this can return CameraGeometry::Duration(0). + template + template + Duration CameraGeometry::temporalOffset( + const Eigen::MatrixBase & keypoint) const { + return _shutter.temporalOffset(keypoint); + } + + template + Duration CameraGeometry::temporalOffset( + const Eigen::VectorXd& keypoint) const { + return _shutter.temporalOffset(keypoint); + } + + ////////////////////////////////////////////////////////////// + // VALIDITY TESTING + ////////////////////////////////////////////////////////////// + + template + template + bool CameraGeometry::isValid( + const Eigen::MatrixBase & keypoint) const { + return _mask.isValid(keypoint) && _projection.isValid(keypoint); + } + + template + template + bool CameraGeometry::isEuclideanVisible( + const Eigen::MatrixBase & p) const { + keypoint_t k; + bool success = _projection.euclideanToKeypoint(p, k); + + return success && _mask.isValid(k); + } + + template + template + bool CameraGeometry::isHomogeneousVisible( + const Eigen::MatrixBase & ph) const { + keypoint_t k; + bool success = _projection.homogeneousToKeypoint(ph, k); + + return success && _mask.isValid(k); + + } + + template + bool CameraGeometry::isProjectionInvertible() const { + return _projection.isProjectionInvertible(); + } + + template + bool CameraGeometry::isBinaryEqual( + const CameraGeometry & rhs) const { + return SM_CHECKMEMBERSSAME(rhs, _id) && SM_CHECKMEMBERSSAME(rhs, _projection) + && SM_CHECKMEMBERSSAME(rhs, _shutter) && SM_CHECKMEMBERSSAME(rhs, _mask); + } + + template + CameraGeometry CameraGeometry::getTestGeometry() { + CameraGeometry camera(projection_t::getTestProjection(), + shutter_t::getTestShutter(), mask_t()); + camera.setId(CameraId(rand())); + return camera; + } + + /// \brief initialize the intrinsics based on list of views of a gridded calibration target + + template + bool CameraGeometry::initializeIntrinsics(const std::vector &observations) { + return _projection.initializeIntrinsics(observations); + } + + /// \brief estimate the transformation of the camera with respect to the calibration target + template + bool CameraGeometry::estimateTransformation( + const GridCalibrationTargetObservation & obs, + sm::kinematics::Transformation & out_T_t_c) const { + return _projection.estimateTransformation(obs, out_T_t_c); + } + + template + boost::shared_ptr::frame_t> CameraGeometry::createUninitializedFrame() const { + + boost::shared_ptr < frame_t > frame(new frame_t); + return frame; + + } + + template + boost::shared_ptr CameraGeometry::createUninitializedFrameBase() const { + + boost::shared_ptr < FrameBase > frame(new frame_t); + return frame; + + } + + template + void CameraGeometry::print(std::ostream & out) { + std::cout << "Printing!\n"; + Eigen::MatrixXd P; + _projection.getParameters(P); + out << "Projection: " << P.transpose() << std::endl; + _projection.distortion().getParameters(P); + if (P.rows() > 0 && P.cols() > 0) { + out << "Distortion: " << P.transpose() << std::endl; + } + } + + /// \brief the minimal dimensions of the projection parameters + template + int CameraGeometry::minimalDimensionsProjection() const { + return _projection.minimalDimensions(); + } + + /// \brief the minimal dimensions of the distortion parameters + template + int CameraGeometry::minimalDimensionsDistortion() const { + return _projection.distortion().minimalDimensions(); + } + + template + int CameraGeometry::minimalDimensionsShutter() const { + return _shutter.minimalDimensions(); + } + + /// \brief update the intrinsics + template + void CameraGeometry::update(const double * v, bool estimateProjection, + bool estimateDistortion, + bool estimateShutter) { + if (estimateProjection) { + _projection.update(v); + v += _projection.minimalDimensions(); + } + + if (estimateDistortion) { + _projection.distortion().update(v); + v += _projection.distortion().minimalDimensions(); + } + + if (estimateShutter) { + _shutter.update(v); + } + } + + /// \brief Get the total number of dimensions of the intrinsic parameters + template + int CameraGeometry::minimalDimensions(bool estimateProjection, + bool estimateDistortion, + bool estimateShutter) const { + int dim = 0; + if (estimateProjection) { + dim += _projection.minimalDimensions(); + } + + if (estimateDistortion) { + dim += _projection.distortion().minimalDimensions(); + } + + if (estimateShutter) { + dim += _shutter.minimalDimensions(); + } + return dim; + } + + /// \brief get the intrinsic parameters. + template + void CameraGeometry::getParameters(Eigen::MatrixXd & params, + bool estimateProjection, + bool estimateDistortion, + bool estimateShutter) const { + Eigen::MatrixXd Pp, Pd, Ps; + int rows = 0, cols = 0; + if (estimateProjection) { + _projection.getParameters(Pp); + rows += Pp.rows(); + cols = std::max(cols, (int) Pp.cols()); + } + + if (estimateDistortion) { + _projection.distortion().getParameters(Pd); + rows += Pd.rows(); + cols = std::max(cols, (int) Pd.cols()); + } + + if (estimateShutter) { + _shutter.getParameters(Ps); + rows += Ps.rows(); + cols = std::max(cols, (int) Ps.cols()); + } + + params = Eigen::MatrixXd::Zero(rows, cols); + int row = 0; + + if (estimateProjection) { + params.block(row, 0, Pp.rows(), Pp.cols()) = Pp; + row += Pp.rows(); + } + + if (estimateDistortion) { + params.block(row, 0, Pd.rows(), Pd.cols()) = Pd; + row += Pd.rows(); + } + + if (estimateShutter) { + params.block(row, 0, Ps.rows(), Ps.cols()) = Ps; + row += Ps.rows(); + } + + } + + /// \brief set the intrinsic parameters. + template + void CameraGeometry::setParameters(const Eigen::MatrixXd & params, + bool estimateProjection, + bool estimateDistortion, + bool estimateShutter) { + // Ugh...I wish I had used a vector, not a matrix. + int row = 0; + if (estimateProjection) { + Eigen::Vector2i sz = _projection.parameterSize(); + _projection.setParameters(params.block(row, 0, sz[0], sz[1])); + row += sz[0]; + } + + if (estimateDistortion) { + Eigen::Vector2i sz = _projection.distortion().parameterSize(); + _projection.distortion().setParameters(params.block(row, 0, sz[0], sz[1])); + row += sz[0]; + } + + if (estimateShutter) { + Eigen::Vector2i sz = _shutter.parameterSize(); + _shutter.setParameters(params.block(row, 0, sz[0], sz[1])); + row += sz[0]; + } + + } + + /// \brief return the Jacobian of the projection with respect to the intrinsics. + template + void CameraGeometry::euclideanToKeypointIntrinsicsJacobian( + const Eigen::Vector3d & p, Eigen::MatrixXd & outJi, bool estimateProjection, + bool estimateDistortion, bool estimateShutter) const { + // Here I have to stack the answers. The resulting matrix should be + // KeypointDimension x minimalDimensions() + outJi = Eigen::MatrixXd::Zero( + KeypointDimension, + minimalDimensions(estimateProjection, estimateDistortion, + estimateShutter)); + + int col = 0; + if (estimateProjection) { + Eigen::MatrixXd J; + _projection.euclideanToKeypointIntrinsicsJacobian(p, J); + outJi.block(col, 0, KeypointDimension, _projection.minimalDimensions()) = J; + col += _projection.minimalDimensions(); + } + + if (estimateDistortion) { + Eigen::MatrixXd J; + _projection.euclideanToKeypointDistortionJacobian(p, J); + outJi.block(col, 0, KeypointDimension, + _projection.distortion().minimalDimensions()) = J; + col += _projection.distortion().minimalDimensions(); + } + + if (estimateShutter) { + // Uh...nothing. The shutter acts on the keypoint time. + // At least for any example I can think of. + } + + } + + /// \brief return the Jacobian of the projection with respect to the intrinsics. + template + void CameraGeometry::homogeneousToKeypointIntrinsicsJacobian( + const Eigen::Vector4d & p, Eigen::MatrixXd & outJi, bool estimateProjection, + bool estimateDistortion, bool estimateShutter) const { + // Here I have to stack the answers. The resulting matrix should be + // KeypointDimension x minimalDimensions() + outJi = Eigen::MatrixXd::Zero( + KeypointDimension, + minimalDimensions(estimateProjection, estimateDistortion, + estimateShutter)); + + int col = 0; + if (estimateProjection) { + Eigen::MatrixXd J; + _projection.homogeneousToKeypointIntrinsicsJacobian(p, J); + outJi.block(0, col, KeypointDimension, _projection.minimalDimensions()) = J; + col += _projection.minimalDimensions(); + } + + if (estimateDistortion) { + Eigen::MatrixXd J; + _projection.homogeneousToKeypointDistortionJacobian(p, J); + outJi.block(0, col, KeypointDimension, + _projection.distortion().minimalDimensions()) = J; + col += _projection.distortion().minimalDimensions(); + } + + if (estimateShutter) { + // Uh...nothing. The shutter acts on the keypoint time. + // At least for any example I can think of. + } + + } + + /// \brief return the temporal offset with respect to the intrinsics. + template + void CameraGeometry::temporalOffsetIntrinsicsJacobian( + const Eigen::VectorXd & keypoint, Eigen::MatrixXd & outJi, + bool estimateProjection, bool estimateDistortion, + bool estimateShutter) const { + // Here I have to stack the answers. The resulting matrix should be + // 1 x totalparameterSize + outJi = Eigen::MatrixXd::Zero( + 1, + minimalDimensions(estimateProjection, estimateDistortion, + estimateShutter)); + + int col = 0; + if (estimateProjection) { + col += _projection.minimalDimensions(); + } + + if (estimateDistortion) { + col += _projection.distortion().minimalDimensions(); + } + + if (estimateShutter) { + _shutter.temporalOffsetIntrinsicsJacobian( + keypoint, outJi.block(0, col, 1, _shutter.minimalDimensions())); + } + } + + } // namespace cameras } // namespace aslam diff --git a/aslam_cv/aslam_cameras/include/aslam/cameras/implementation/OmniProjection.hpp b/aslam_cv/aslam_cameras/include/aslam/cameras/implementation/OmniProjection.hpp index 46f9e17d3..084dafde3 100644 --- a/aslam_cv/aslam_cameras/include/aslam/cameras/implementation/OmniProjection.hpp +++ b/aslam_cv/aslam_cameras/include/aslam/cameras/implementation/OmniProjection.hpp @@ -366,7 +366,7 @@ bool OmniProjection::keypointToHomogeneous( Eigen::MatrixBase & Jk = const_cast &>(outJk); - Jk.derived().resize(2, 4); + Jk.derived().resize(4, 2); Jk.setZero(); Eigen::MatrixBase & p = diff --git a/aslam_cv/aslam_cameras/include/aslam/cameras/implementation/PinholeProjection.hpp b/aslam_cv/aslam_cameras/include/aslam/cameras/implementation/PinholeProjection.hpp index 7943af000..51f237e47 100644 --- a/aslam_cv/aslam_cameras/include/aslam/cameras/implementation/PinholeProjection.hpp +++ b/aslam_cv/aslam_cameras/include/aslam/cameras/implementation/PinholeProjection.hpp @@ -305,9 +305,9 @@ bool PinholeProjection::keypointToHomogeneous( EIGEN_STATIC_ASSERT_MATRIX_SPECIFIC_SIZE_OR_DYNAMIC( Eigen::MatrixBase, 4, 2); - Eigen::MatrixBase & Jk = - const_cast &>(outJk); - Jk.derived().resize(2, 4); + Eigen::MatrixBase & Jk = const_cast &>(outJk); + + Jk.derived().resize(4, 2); Jk.setZero(); Eigen::MatrixBase & p = diff --git a/aslam_cv/aslam_cameras/include/aslam/cameras/test/CameraGeometryTestHarness.hpp b/aslam_cv/aslam_cameras/include/aslam/cameras/test/CameraGeometryTestHarness.hpp index fe3b032df..ac4bfe873 100644 --- a/aslam_cv/aslam_cameras/include/aslam/cameras/test/CameraGeometryTestHarness.hpp +++ b/aslam_cv/aslam_cameras/include/aslam/cameras/test/CameraGeometryTestHarness.hpp @@ -380,6 +380,6 @@ class CameraGeometryTestHarness { }; -} // namespace cameras +} // namespace cameras } // namespace asl diff --git a/aslam_cv/aslam_cameras/src/PinholeCameraGeometry.cpp b/aslam_cv/aslam_cameras/src/PinholeCameraGeometry.cpp index 21521283f..ec425af81 100644 --- a/aslam_cv/aslam_cameras/src/PinholeCameraGeometry.cpp +++ b/aslam_cv/aslam_cameras/src/PinholeCameraGeometry.cpp @@ -89,9 +89,9 @@ PinholeCameraGeometry PinholeCameraGeometry::createDistortedTestGeometry() { return PinholeCameraGeometry(400, 400, 320, 240, 640, 480, 0.2, 0.2, 0.2, 0.2); } -/** +/** * \brief Apply distortion to input point (from the normalised plane) - * + * * \param mx_u undistorted x coordinate of point on the normalised plane * \param my_u undistorted y coordinate of point on the normalised plane * \param dx return value, to obtain the distorted point : mx_d = mx_u+dx_u @@ -110,7 +110,7 @@ void PinholeCameraGeometry::distortion(double mx_u, double my_u, double *dx_u, *dy_u = my_u * rad_dist_u + 2 * _p2 * mxy_u + _p1 * (rho2_u + 2 * my2_u); } -/** +/** * \brief Apply distortion to input point (from the normalised plane) * and calculate jacobian * @@ -203,6 +203,6 @@ PinholeCameraGeometry::traits_t::intrinsics_t PinholeCameraGeometry::getIntrinsi return intrinsics; } -} // namespace cameras +} // namespace cameras } // namespace aslam diff --git a/aslam_cv/aslam_cv_backend/CMakeLists.txt b/aslam_cv/aslam_cv_backend/CMakeLists.txt index 4b775dda9..05bc53100 100644 --- a/aslam_cv/aslam_cv_backend/CMakeLists.txt +++ b/aslam_cv/aslam_cv_backend/CMakeLists.txt @@ -5,13 +5,10 @@ find_package(catkin_simple REQUIRED) catkin_simple() - cs_add_library(${PROJECT_NAME} - src/ReprojectionError.cpp src/GridCalibrationTargetDesignVariableContainer.cpp - src/CameraGeometryDesignVariableContainer.cpp - src/ScalarExpressionNodeKeypointTime.cpp -# src/NCameraSystemDesignVariableContainer.cpp + # src/CameraGeometryDesignVariableContainer.cpp + # src/NCameraSystemDesignVariableContainer.cpp ) # Avoid clash with tr1::tuple: https://code.google.com/p/googletest/source/browse/trunk/README?r=589#257 @@ -19,9 +16,11 @@ add_definitions(-DGTEST_USE_OWN_TR1_TUPLE=0) catkin_add_gtest(${PROJECT_NAME}_test test/test_main.cpp - test/testReprojectionError.cpp ) +find_package(Boost REQUIRED COMPONENTS system) + +target_link_libraries(${PROJECT_NAME} ${Boost_LIBRARIES}) target_link_libraries(${PROJECT_NAME}_test ${PROJECT_NAME}) cs_install() diff --git a/aslam_cv/aslam_cv_backend/include/aslam/CameraGeometryDesignVariableContainer.hpp b/aslam_cv/aslam_cv_backend/include/aslam/CameraGeometryDesignVariableContainer.hpp index 970ca4c43..709b56ad8 100644 --- a/aslam_cv/aslam_cv_backend/include/aslam/CameraGeometryDesignVariableContainer.hpp +++ b/aslam_cv/aslam_cv_backend/include/aslam/CameraGeometryDesignVariableContainer.hpp @@ -8,6 +8,8 @@ #include #include + +/// \remarks This class is neither in use nor compiled. namespace aslam { class ReprojectionError; diff --git a/aslam_cv/aslam_cv_backend/include/aslam/NCameraSystemDesignVariableContainer.hpp b/aslam_cv/aslam_cv_backend/include/aslam/NCameraSystemDesignVariableContainer.hpp index 25505a63f..f83ed95ed 100644 --- a/aslam_cv/aslam_cv_backend/include/aslam/NCameraSystemDesignVariableContainer.hpp +++ b/aslam_cv/aslam_cv_backend/include/aslam/NCameraSystemDesignVariableContainer.hpp @@ -11,6 +11,7 @@ namespace aslam { +/// \remarks This class is neither in use nor compiled. class NCameraSystemDesignVariableContainer { public: diff --git a/aslam_cv/aslam_cv_backend/include/aslam/ReprojectionError.hpp b/aslam_cv/aslam_cv_backend/include/aslam/ReprojectionError.hpp deleted file mode 100644 index a2dc7efa3..000000000 --- a/aslam_cv/aslam_cv_backend/include/aslam/ReprojectionError.hpp +++ /dev/null @@ -1,67 +0,0 @@ -#ifndef ASLAM_RE_REFACTOR_HPP -#define ASLAM_RE_REFACTOR_HPP - -#include -#include -#include -#include - -namespace aslam { - -class ReprojectionError : public backend::ErrorTermDs { - public: - EIGEN_MAKE_ALIGNED_OPERATOR_NEW - - ReprojectionError(const Eigen::VectorXd & y, const Eigen::MatrixXd & invR, - backend::HomogeneousExpression p_c, - cameras::CameraGeometryBase * cam); - - ReprojectionError(const Eigen::VectorXd & y, const Eigen::MatrixXd & invR, - backend::HomogeneousExpression p_c, - CameraGeometryDesignVariableContainer * camDvc); - - virtual ~ReprojectionError(); - - /// \brief evaluate the error term and return the weighted squared error e^T invR e - virtual double evaluateErrorImplementation(); - - /// \brief evaluate the Jacobians - virtual void evaluateJacobiansImplementation( - aslam::backend::JacobianContainer & J); - - /// \brief Get the camera - aslam::cameras::CameraGeometryBase * getCamera() const; - - /// \brief get the keypoint measurement - Eigen::VectorXd getKeypoint() const; - - /// \brief get the projection - Eigen::VectorXd getProjection(); - - /// \brief was the projection successful? - bool getProjectionSuccess(); - - /// \brief get the point expressed in the camera frame - aslam::backend::HomogeneousExpression getPoint() const; - - bool getReprojectedPoint(Eigen::VectorXd& hat_y); - - /// \brief is the measurement enabled? - bool isEnabled() const; - /// \brief is the error term valid? - bool isValid() const; - - void setValid(bool valid); - - private: - Eigen::VectorXd _y; - aslam::backend::HomogeneousExpression _p_c; - aslam::cameras::CameraGeometryBase * _cam; - CameraGeometryDesignVariableContainer * _camDvc; - bool _enabled; - bool _valid; -}; - -} // namespace aslam - -#endif /* ASLAM_RE_HPP */ diff --git a/aslam_cv/aslam_cv_backend/include/aslam/ScalarExpressionNodeKeypointTime.hpp b/aslam_cv/aslam_cv_backend/include/aslam/ScalarExpressionNodeKeypointTime.hpp deleted file mode 100644 index dce615801..000000000 --- a/aslam_cv/aslam_cv_backend/include/aslam/ScalarExpressionNodeKeypointTime.hpp +++ /dev/null @@ -1,36 +0,0 @@ -#ifndef ASLAM_SCALAR_EXPRESSION_NODE_KEYPOINT_TIME_HPP -#define ASLAM_SCALAR_EXPRESSION_NODE_KEYPOINT_TIME_HPP - -#include -#include -namespace aslam { - -class ScalarExpressionNodeKeypointTime : - public aslam::backend::ScalarExpressionNode { - public: - ScalarExpressionNodeKeypointTime( - const aslam::Time & stamp, - const Eigen::VectorXd & y, - boost::shared_ptr< - backend::DesignVariableAdapter > dv); - virtual ~ScalarExpressionNodeKeypointTime(); - - virtual double toScalarImplementation() const; - virtual void evaluateJacobiansImplementation( - backend::JacobianContainer & outJacobians) const; - virtual void evaluateJacobiansImplementation( - backend::JacobianContainer & outJacobians, - const Eigen::MatrixXd & applyChainRule) const; - virtual void getDesignVariablesImplementation( - backend::DesignVariable::set_t & designVariables) const; - - private: - aslam::Time _stamp; - Eigen::VectorXd _y; - boost::shared_ptr< - backend::DesignVariableAdapter > _dv; -}; - -} // namespace aslam - -#endif /* ASLAM_SCALAR_EXPRESSION_NODE_KEYPOINT_TIME_HPP */ diff --git a/aslam_optimizer/aslam_backend_expressions/include/aslam/backend/CameraDesignVariable.hpp b/aslam_cv/aslam_cv_backend/include/aslam/backend/CameraDesignVariable.hpp similarity index 74% rename from aslam_optimizer/aslam_backend_expressions/include/aslam/backend/CameraDesignVariable.hpp rename to aslam_cv/aslam_cv_backend/include/aslam/backend/CameraDesignVariable.hpp index b9c7bee34..c59dbd283 100644 --- a/aslam_optimizer/aslam_backend_expressions/include/aslam/backend/CameraDesignVariable.hpp +++ b/aslam_cv/aslam_cv_backend/include/aslam/backend/CameraDesignVariable.hpp @@ -4,13 +4,18 @@ #include #include #include - +#include +#include +#include +#include +#include namespace aslam { namespace backend { template - class CameraDesignVariable + class CameraDesignVariable: + public boost::enable_shared_from_this > { public: SM_DEFINE_EXCEPTION(Exception, std::runtime_error); @@ -42,6 +47,14 @@ namespace aslam { boost::shared_ptr camera() { return _camera; } + /// \brief Get the keypoint time as an expression. If the shutter + /// parameters are being estimated, this will be hooked up + /// to the camera design variable + backend::ScalarExpression keypointTime(const aslam::Time & imageStamp, const Eigen::VectorXd & y); + + // get the temporal offset of a given keypoint y + backend::ScalarExpression temporalOffset(const Eigen::VectorXd & y); + private: boost::shared_ptr _camera; diff --git a/aslam_cv/aslam_cv_backend/include/aslam/backend/ScalarExpressionNodeKeypointTime.hpp b/aslam_cv/aslam_cv_backend/include/aslam/backend/ScalarExpressionNodeKeypointTime.hpp new file mode 100644 index 000000000..90f5ba228 --- /dev/null +++ b/aslam_cv/aslam_cv_backend/include/aslam/backend/ScalarExpressionNodeKeypointTime.hpp @@ -0,0 +1,55 @@ +#ifndef ASLAM_SCALAR_EXPRESSION_NODE_KEYPOINT_TIME_HPP +#define ASLAM_SCALAR_EXPRESSION_NODE_KEYPOINT_TIME_HPP + +#include +#include +#include + +// forward definition of the camera design variable +namespace aslam { + namespace backend { + template + class CameraDesignVariable; + } +} + +namespace aslam { + namespace backend { + + template + class ScalarExpressionNodeKeypointTime : + public aslam::backend::ScalarExpressionNode + { + + typedef CAMERA_T camera_t; + + public: + ScalarExpressionNodeKeypointTime( + const aslam::Time & stamp, + const Eigen::VectorXd & y, + boost::shared_ptr > dv + ); + virtual ~ScalarExpressionNodeKeypointTime(); + + virtual double toScalarImplementation() const; + virtual void evaluateJacobiansImplementation(backend::JacobianContainer & outJacobians) const; + virtual void evaluateJacobiansImplementation( + backend::JacobianContainer & outJacobians, + const Eigen::MatrixXd & applyChainRule + ) const; + virtual void getDesignVariablesImplementation( + backend::DesignVariable::set_t & designVariables + ) const; + + private: + aslam::Time _stamp; + Eigen::VectorXd _y; + boost::shared_ptr > _dv; + }; + + } // namespace backend +} // namespace aslam + +#include "implementation/ScalarExpressionNodeKeypointTime.hpp" + +#endif /* ASLAM_SCALAR_EXPRESSION_NODE_KEYPOINT_TIME_HPP */ diff --git a/aslam_optimizer/aslam_backend_expressions/include/aslam/backend/implementation/CameraDesignVariable.hpp b/aslam_cv/aslam_cv_backend/include/aslam/backend/implementation/CameraDesignVariable.hpp similarity index 76% rename from aslam_optimizer/aslam_backend_expressions/include/aslam/backend/implementation/CameraDesignVariable.hpp rename to aslam_cv/aslam_cv_backend/include/aslam/backend/implementation/CameraDesignVariable.hpp index 25c5e962c..9b844714a 100644 --- a/aslam_optimizer/aslam_backend_expressions/include/aslam/backend/implementation/CameraDesignVariable.hpp +++ b/aslam_cv/aslam_cv_backend/include/aslam/backend/implementation/CameraDesignVariable.hpp @@ -80,6 +80,32 @@ namespace aslam { } + template + backend::ScalarExpression CameraDesignVariable::keypointTime( + const aslam::Time & imageStamp, + const Eigen::VectorXd & y + ) { + if (_shutterDv->isActive()) { + boost::shared_ptr root( + new ScalarExpressionNodeKeypointTime( + imageStamp, + y, + this->shared_from_this() + ) + ); + return backend::ScalarExpression(root); + } else { + return backend::ScalarExpression( + (imageStamp + _camera->temporalOffset(y)).toSec() + ); + } + } + + template + backend::ScalarExpression CameraDesignVariable::temporalOffset( + const Eigen::VectorXd & y) { + return keypointTime(aslam::Time(), y); + } } // backend diff --git a/aslam_cv/aslam_cv_backend/include/aslam/backend/implementation/ScalarExpressionNodeKeypointTime.hpp b/aslam_cv/aslam_cv_backend/include/aslam/backend/implementation/ScalarExpressionNodeKeypointTime.hpp new file mode 100644 index 000000000..1b72fa2f3 --- /dev/null +++ b/aslam_cv/aslam_cv_backend/include/aslam/backend/implementation/ScalarExpressionNodeKeypointTime.hpp @@ -0,0 +1,59 @@ +namespace aslam { + namespace backend { + + template + ScalarExpressionNodeKeypointTime::ScalarExpressionNodeKeypointTime( + const aslam::Time & stamp, + const Eigen::VectorXd & y, + boost::shared_ptr > dv + ) + : _stamp(stamp), + _y(y), + _dv(dv) + { + SM_ASSERT_TRUE(std::runtime_error, _dv.get() != NULL, + "Unable to intialize with a null design variable"); + } + + template + ScalarExpressionNodeKeypointTime::~ScalarExpressionNodeKeypointTime() { + + } + + template + double ScalarExpressionNodeKeypointTime::toScalarImplementation() const { + return (_stamp + _dv->camera()->temporalOffset(_y)).toSec(); + } + + template + void ScalarExpressionNodeKeypointTime::evaluateJacobiansImplementation( + backend::JacobianContainer & outJacobians + ) const{ + Eigen::MatrixXd Ji; + + Ji.resize(1, 1); + Ji(0, 0) = _y[1]; + outJacobians.add(_dv->shutterDesignVariable().get(), Ji); + } + + template + void ScalarExpressionNodeKeypointTime::evaluateJacobiansImplementation( + backend::JacobianContainer & outJacobians, + const Eigen::MatrixXd & applyChainRule + ) const { + Eigen::MatrixXd Ji; + + Ji.resize(1, 1); + Ji(0, 0) = _y[1]; + outJacobians.add(_dv->shutterDesignVariable().get(), applyChainRule * Ji); + } + + template + void ScalarExpressionNodeKeypointTime::getDesignVariablesImplementation( + backend::DesignVariable::set_t & designVariables + ) const { + designVariables.insert(_dv->shutterDesignVariable().get()); + } + + } // namespace backend +} // namespace aslam diff --git a/aslam_cv/aslam_cv_backend/package.xml b/aslam_cv/aslam_cv_backend/package.xml index 0f06de544..3e2e87ed5 100644 --- a/aslam_cv/aslam_cv_backend/package.xml +++ b/aslam_cv/aslam_cv_backend/package.xml @@ -8,7 +8,7 @@ BSD catkin catkin_simple - + sm_common sm_boost sm_kinematics @@ -20,7 +20,7 @@ aslam_backend aslam_camera_system - aslam_backend_expressions + aslam_backend_expressions aslam_time aslam_cameras diff --git a/aslam_cv/aslam_cv_backend/src/CameraGeometryDesignVariableContainer.cpp b/aslam_cv/aslam_cv_backend/src/CameraGeometryDesignVariableContainer.cpp index 7214e6362..a9c41a30d 100644 --- a/aslam_cv/aslam_cv_backend/src/CameraGeometryDesignVariableContainer.cpp +++ b/aslam_cv/aslam_cv_backend/src/CameraGeometryDesignVariableContainer.cpp @@ -2,6 +2,8 @@ #include #include #include + +/// \remarks This file is neither in use nor compiled. namespace aslam { CameraGeometryDesignVariableContainer::CameraGeometryDesignVariableContainer( diff --git a/aslam_cv/aslam_cv_backend/src/NCameraSystemDesignVariableContainer.cpp b/aslam_cv/aslam_cv_backend/src/NCameraSystemDesignVariableContainer.cpp index 9ded8e148..224df36de 100644 --- a/aslam_cv/aslam_cv_backend/src/NCameraSystemDesignVariableContainer.cpp +++ b/aslam_cv/aslam_cv_backend/src/NCameraSystemDesignVariableContainer.cpp @@ -2,6 +2,7 @@ #include #include +/// \remarks This file is neither in use nor compiled. namespace aslam { NCameraSystemDesignVariableContainer::NCameraSystemDesignVariableContainer( diff --git a/aslam_cv/aslam_cv_backend/src/ReprojectionError.cpp b/aslam_cv/aslam_cv_backend/src/ReprojectionError.cpp deleted file mode 100644 index b391a889d..000000000 --- a/aslam_cv/aslam_cv_backend/src/ReprojectionError.cpp +++ /dev/null @@ -1,159 +0,0 @@ -#include -#include - -namespace aslam { - -ReprojectionError::ReprojectionError(const Eigen::VectorXd & y, - const Eigen::MatrixXd & invR, - backend::HomogeneousExpression p_c, - aslam::cameras::CameraGeometryBase * cam) - : ErrorTermDs(y.size()), - _y(y), - _p_c(p_c), - _cam(cam), - _camDvc(NULL), - _enabled(true), - _valid(false) { - - SM_ASSERT_TRUE(std::runtime_error, cam != NULL, "The frame must not be null"); - SM_ASSERT_EQ(std::runtime_error, y.size(), _cam->keypointDimension(), - "The measurement size must match the keypoint dimension"); - SM_ASSERT_EQ( - std::runtime_error, invR.rows(), _cam->keypointDimension(), - "The measurement uncertainty size must match the keypoint dimension"); - SM_ASSERT_EQ( - std::runtime_error, invR.cols(), _cam->keypointDimension(), - "The measurement uncertainty size must match the keypoint dimension"); - setInvR(invR); - setError(Eigen::VectorXd::Zero(_cam->keypointDimension())); - backend::JacobianContainer::set_t dvs; - p_c.getDesignVariables(dvs); - setDesignVariablesIterator(dvs.begin(), dvs.end()); -} - -ReprojectionError::ReprojectionError( - const Eigen::VectorXd & y, const Eigen::MatrixXd & invR, - backend::HomogeneousExpression p_c, - CameraGeometryDesignVariableContainer * camDvc) - : ErrorTermDs(y.size()), - _y(y), - _p_c(p_c), - _camDvc(camDvc), - _enabled(true), - _valid(false) { - SM_ASSERT_TRUE(std::runtime_error, _camDvc != NULL, - "The camera DVC must not be null!"); - _cam = camDvc->camera().get(); - SM_ASSERT_TRUE(std::runtime_error, _cam != NULL, - "The camera must not be null"); - SM_ASSERT_EQ(std::runtime_error, y.size(), _cam->keypointDimension(), - "The measurement size must match the keypoint dimension"); - SM_ASSERT_EQ( - std::runtime_error, invR.rows(), _cam->keypointDimension(), - "The measurement uncertainty size must match the keypoint dimension"); - SM_ASSERT_EQ( - std::runtime_error, invR.cols(), _cam->keypointDimension(), - "The measurement uncertainty size must match the keypoint dimension"); - setInvR(invR); - setError(Eigen::VectorXd::Zero(_cam->keypointDimension())); - backend::JacobianContainer::set_t dvs; - p_c.getDesignVariables(dvs); - _camDvc->getDesignVariables(dvs); - setDesignVariablesIterator(dvs.begin(), dvs.end()); -} - -ReprojectionError::~ReprojectionError() { - -} - -/// \brief evaluate the error term and return the weighted squared error e^T invR e -double ReprojectionError::evaluateErrorImplementation() { - Eigen::VectorXd p = _p_c.toHomogeneous(); - Eigen::VectorXd hat_y; - bool success = _cam->vsHomogeneousToKeypoint(p, hat_y); - if (success) { - _enabled = true; - setError(hat_y - _y); - } else { - // If we ever fail, turn this guy off. - _enabled = false; - // leave the error at the previous value. - } - - return evaluateChiSquaredError(); -} - -/// \brief is the measurement enabled? -bool ReprojectionError::isEnabled() const { - return _enabled; -} - -bool ReprojectionError::isValid() const { - return _valid; -} - -void ReprojectionError::setValid(bool valid) { - _valid = valid; -} - -/// \brief Get the camera -aslam::cameras::CameraGeometryBase * ReprojectionError::getCamera() const { - return _cam; -} - -/// \brief evaluate the Jacobians -void ReprojectionError::evaluateJacobiansImplementation( - aslam::backend::JacobianContainer & _jacobians) { - - Eigen::Vector4d p = _p_c.toHomogeneous(); - if (_enabled) { - Eigen::MatrixXd J; - Eigen::VectorXd hat_y; - _cam->vsHomogeneousToKeypoint(p, hat_y, J); - _p_c.evaluateJacobians(_jacobians, J); - - if (_camDvc) { - _camDvc->evaluateJacobians(_jacobians, p); - } - } else { - // HACK - //Eigen::MatrixXd J = Eigen::MatrixXd::Zero(_cam->keypointDimension(), 4); - //_p_c.evaluateJacobians(_jacobians, J); - _jacobians.clear(); - } - -} - -/// \brief get the keypoint measurement -Eigen::VectorXd ReprojectionError::getKeypoint() const { - return _y; -} - -/// \brief get the keypoint measurement -Eigen::VectorXd ReprojectionError::getProjection() { - Eigen::VectorXd p = _p_c.toHomogeneous(); - Eigen::VectorXd hat_y; - _cam->vsHomogeneousToKeypoint(p, hat_y); - return hat_y; -} - -/// \brief get the keypoint measurement -bool ReprojectionError::getProjectionSuccess() { - Eigen::VectorXd p = _p_c.toHomogeneous(); - Eigen::VectorXd hat_y; - bool success = _cam->vsHomogeneousToKeypoint(p, hat_y); - return success; -} - -/// \brief get the point expressed in the camera frame -aslam::backend::HomogeneousExpression ReprojectionError::getPoint() const { - return _p_c; -} - -bool ReprojectionError::getReprojectedPoint(Eigen::VectorXd& hat_y) { - Eigen::VectorXd p = _p_c.toHomogeneous(); - return _cam->vsHomogeneousToKeypoint(p, hat_y); -} - -} // namespace aslam - diff --git a/aslam_cv/aslam_cv_backend/src/ScalarExpressionNodeKeypointTime.cpp b/aslam_cv/aslam_cv_backend/src/ScalarExpressionNodeKeypointTime.cpp deleted file mode 100644 index ed709cec1..000000000 --- a/aslam_cv/aslam_cv_backend/src/ScalarExpressionNodeKeypointTime.cpp +++ /dev/null @@ -1,47 +0,0 @@ -#include - -namespace aslam { - -ScalarExpressionNodeKeypointTime::ScalarExpressionNodeKeypointTime( - const aslam::Time & stamp, - const Eigen::VectorXd & y, - boost::shared_ptr< - backend::DesignVariableAdapter > dv) - : _stamp(stamp), - _y(y), - _dv(dv) { - SM_ASSERT_TRUE(std::runtime_error, _dv.get() != NULL, - "Unable to intialize with a null design variable"); -} - -ScalarExpressionNodeKeypointTime::~ScalarExpressionNodeKeypointTime() { - -} - -double ScalarExpressionNodeKeypointTime::toScalarImplementation() const { - return (_stamp + _dv->value().camera()->temporalOffset(_y)).toSec(); -} - -void ScalarExpressionNodeKeypointTime::evaluateJacobiansImplementation( - backend::JacobianContainer & outJacobians) const { - Eigen::MatrixXd Ji; - - _dv->value().temporalOffsetIntrinsicsJacobian(_y, Ji); - outJacobians.add(_dv.get(), Ji); -} - -void ScalarExpressionNodeKeypointTime::evaluateJacobiansImplementation( - backend::JacobianContainer & outJacobians, - const Eigen::MatrixXd & applyChainRule) const { - Eigen::MatrixXd Ji; - - _dv->value().temporalOffsetIntrinsicsJacobian(_y, Ji); - outJacobians.add(_dv.get(), applyChainRule * Ji); -} - -void ScalarExpressionNodeKeypointTime::getDesignVariablesImplementation( - backend::DesignVariable::set_t & designVariables) const { - designVariables.insert(_dv.get()); -} - -} // namespace aslam diff --git a/aslam_cv/aslam_cv_backend_python/CMakeLists.txt b/aslam_cv/aslam_cv_backend_python/CMakeLists.txt index 3ad235e2b..e0edcce3f 100644 --- a/aslam_cv/aslam_cv_backend_python/CMakeLists.txt +++ b/aslam_cv/aslam_cv_backend_python/CMakeLists.txt @@ -8,7 +8,7 @@ catkin_simple() add_python_export_library(${PROJECT_NAME} python/aslam_cv_backend src/module.cpp src/GridCalibration.cpp - src/CameraGeometryDesignVariableContainer.cpp + # src/CameraGeometryDesignVariableContainer.cpp # src/NCameraSystemDesignVariableContainer.cpp ) diff --git a/aslam_cv/aslam_cv_backend_python/include/aslam/CameraDesignVariable.hpp b/aslam_cv/aslam_cv_backend_python/include/aslam/CameraDesignVariable.hpp deleted file mode 100644 index 6fd697c35..000000000 --- a/aslam_cv/aslam_cv_backend_python/include/aslam/CameraDesignVariable.hpp +++ /dev/null @@ -1,72 +0,0 @@ -#ifndef ASLAM_BACKEND_CAMERA_VARIABLE_HPP -#define ASLAM_BACKEND_CAMERA_VARIABLE_HPP - -#include -#include -#include - -namespace aslam { -namespace backend { - -template -class CameraDesignVariable { - public: - - typedef CAMERA_T camera_t; - typedef typename camera_t::keypoint_t keypoint_t; - typedef typename camera_t::projection_t projection_t; - typedef typename camera_t::projection_t::distortion_t distortion_t; - typedef typename camera_t::shutter_t shutter_t; - - CameraDesignVariable(const boost::shared_ptr & camera); - ~CameraDesignVariable(); - - Eigen::VectorXd euclideanToKeypoint(Eigen::Vector3d p); - Eigen::VectorXd homogeneousToKeypoint(Eigen::Vector4d ph); - - void evaluateJacobians(JacobianContainer & outJacobians, - Eigen::Vector4d ph) const; - void evaluateJacobians(JacobianContainer & outJacobians, - const Eigen::MatrixXd & applyChainRule, - Eigen::Vector4d ph) const; - - void getDesignVariables(DesignVariable::set_t & designVariables) const; - - void setActive(bool p, bool d, bool s); - - void setTestBlockIndices() { - _projectionDv->setBlockIndex(0); - _distortionDv->setBlockIndex(1); - _shutterDv->setBlockIndex(2); - } - ; - - boost::shared_ptr > projectionDesignVariable() { - return _projectionDv; - } - boost::shared_ptr > distortionDesignVariable() { - return _distortionDv; - } - boost::shared_ptr > shutterDesignVariable() { - return _shutterDv; - } - - boost::shared_ptr camera() { - return _camera; - } - - private: - boost::shared_ptr _camera; - - boost::shared_ptr > _projectionDv; - boost::shared_ptr > _distortionDv; - boost::shared_ptr > _shutterDv; - -}; - -} // backend -} // aslam - -#include "implementation/CameraDesignVariable.hpp" - -#endif /* ASLAM_BACKEND_CAMERA_VARIABLE_HPP */ diff --git a/aslam_cv/aslam_cv_backend_python/include/aslam/ExportCameraDesignVariable.hpp b/aslam_cv/aslam_cv_backend_python/include/aslam/ExportCameraDesignVariable.hpp index 17214c15c..8c3401343 100644 --- a/aslam_cv/aslam_cv_backend_python/include/aslam/ExportCameraDesignVariable.hpp +++ b/aslam_cv/aslam_cv_backend_python/include/aslam/ExportCameraDesignVariable.hpp @@ -4,6 +4,7 @@ #include #include #include +#include namespace aslam { namespace python { @@ -18,19 +19,20 @@ void exportCameraDesignVariables(std::string name) { boost::python::class_, boost::shared_ptr > >( (name + "DesignVariable").c_str(), - boost::python::init >()).def( - "euclideanToKeypoint", &CameraDesignVariable::euclideanToKeypoint).def( - "homogeneousToKeypoint", &CameraDesignVariable::homogeneousToKeypoint) - .def("setActive", &CameraDesignVariable::setActive).def( - "getDesignVariables", - &aslam::python::getDesignVariables >).def( - "projectionDesignVariable", - &CameraDesignVariable::projectionDesignVariable).def( - "distortionDesignVariable", - &CameraDesignVariable::distortionDesignVariable).def( - "shutterDesignVariable", &CameraDesignVariable::shutterDesignVariable) - .def("camera", &CameraDesignVariable::camera); - + boost::python::init >()) + .def("euclideanToKeypoint", &CameraDesignVariable::euclideanToKeypoint) + .def("homogeneousToKeypoint", &CameraDesignVariable::homogeneousToKeypoint) + .def("setActive", &CameraDesignVariable::setActive) + .def("getDesignVariables", &aslam::python::getDesignVariables >) + .def("projectionDesignVariable", &CameraDesignVariable::projectionDesignVariable) + .def("distortionDesignVariable", &CameraDesignVariable::distortionDesignVariable) + .def("shutterDesignVariable", &CameraDesignVariable::shutterDesignVariable) + .def("camera", &CameraDesignVariable::camera) + .def("keypointTime", &CameraDesignVariable::keypointTime) + .def("temporalOffset", &CameraDesignVariable::temporalOffset) + ; + + aslam::python::exportScalarExpressionNodeKeypointTime(name); } template diff --git a/aslam_cv/aslam_cv_backend_python/include/aslam/ExportReprojectionError.hpp b/aslam_cv/aslam_cv_backend_python/include/aslam/ExportReprojectionError.hpp index 10dea84b8..58e5055e6 100644 --- a/aslam_cv/aslam_cv_backend_python/include/aslam/ExportReprojectionError.hpp +++ b/aslam_cv/aslam_cv_backend_python/include/aslam/ExportReprojectionError.hpp @@ -3,7 +3,7 @@ #include #include #include -//#include +#include #include #include #include @@ -60,31 +60,43 @@ void exportReprojectionError(const std::string & camName) { } -// \todo Find a place for the covariance reprojection error... -// template -// void exportCovarianceReprojectionError(const std::string & camName) -// { -// std::string name = camName + "ReprojectionErrorAdaptiveCovariance"; -// using namespace boost::python; -// using namespace aslam; -// using namespace aslam::backend; -// typedef CAMERA_GEOMETRY_T geometry_t; -// typedef DescriptorBase descriptor_t; -// typedef Frame frame_t; -// typedef typename frame_t::keypoint_t keypoint_t; +template +void exportCovarianceReprojectionError(const std::string & camName) +{ + std::string name = camName + "ReprojectionErrorAdaptiveCovariance"; + using namespace boost::python; + using namespace aslam; + using namespace aslam::backend; + typedef CAMERA_GEOMETRY_T geometry_t; + typedef DescriptorBase descriptor_t; + typedef Frame frame_t; + typedef typename frame_t::keypoint_t keypoint_t; -// class_< CovarianceReprojectionError, boost::shared_ptr >, bases< ErrorTerm > >( name.c_str(), -// init, aslam::splines::BSplinePoseDesignVariable*, aslam::backend::Scalar* >( (name + "( frame, keypointIndex, homogeneousPointExpression, CameraDesignVariable, bsplineDesignVariable, lineDelayDv)").c_str()) ) -// .def("observationTime", &CovarianceReprojectionError::observationTime) -// .def("covarianceMatrix", &CovarianceReprojectionError::covarianceMatrix) -// ; + class_< + CovarianceReprojectionError, + boost::shared_ptr + >,bases< ErrorTerm > >( + name.c_str(), + init< + const frame_t *, + int, + HomogeneousExpression, + CameraDesignVariable, + aslam::splines::BSplinePoseDesignVariable* + > + ( + (name + "( frame, keypointIndex, homogeneousPointExpression, CameraDesignVariable, bsplineDesignVariable)").c_str() + ) + ) + .def("observationTime", &CovarianceReprojectionError::observationTime) + .def("covarianceMap", &CovarianceReprojectionError::covarianceMap) + ; -// } +} template void exportReprojectionErrors(const std::string & camName) { exportReprojectionError(camName); - //exportCovarianceReprojectionError(camName); } } // namespace python diff --git a/aslam_cv/aslam_cv_backend_python/include/aslam/ExportScalarExpressionNodeKeypointTime.hpp b/aslam_cv/aslam_cv_backend_python/include/aslam/ExportScalarExpressionNodeKeypointTime.hpp new file mode 100644 index 000000000..ad952b1c2 --- /dev/null +++ b/aslam_cv/aslam_cv_backend_python/include/aslam/ExportScalarExpressionNodeKeypointTime.hpp @@ -0,0 +1,31 @@ +// It is extremely important to use this header +// if you are using the numpy_eigen interface +#include +#include +#include + +namespace aslam { + namespace python { + + template + void exportScalarExpressionNodeKeypointTime(std::string name) { + using namespace boost::python; + using namespace aslam; + + class_< + ScalarExpressionNodeKeypointTime, + boost::shared_ptr > + >( + (name + "ScalarExpressionNodeKeypointTime").c_str(), + init< + const aslam::Time &, + const Eigen::VectorXd &, + boost::shared_ptr > + >( + "ScalarExpressionNodeKeypointTime(frame timestamp, keypoint, camera design variable)" + ) + ) + .def("toScalar", &ScalarExpressionNodeKeypointTime::toScalar); + } + } +} diff --git a/aslam_cv/aslam_cv_backend_python/include/aslam/implementation/CameraDesignVariable.hpp b/aslam_cv/aslam_cv_backend_python/include/aslam/implementation/CameraDesignVariable.hpp deleted file mode 100644 index 8025008cf..000000000 --- a/aslam_cv/aslam_cv_backend_python/include/aslam/implementation/CameraDesignVariable.hpp +++ /dev/null @@ -1,99 +0,0 @@ -namespace aslam { -namespace backend { - -template -CameraDesignVariable::CameraDesignVariable( - const boost::shared_ptr & camera) - : _camera(camera) { - // create the design variables: - _projectionDv.reset( - new DesignVariableAdapter(&_camera->projection(), false)); - _distortionDv.reset( - new DesignVariableAdapter( - &((_camera->projection()).distortion()), false)); - _shutterDv.reset( - new DesignVariableAdapter(&_camera->shutter(), false)); -} - -template -CameraDesignVariable::~CameraDesignVariable() { -} - -template -void CameraDesignVariable::setActive(bool p, bool d, bool s) { - _projectionDv->setActive(p); - _distortionDv->setActive(d); - _shutterDv->setActive(s); -} - -template -Eigen::VectorXd CameraDesignVariable::euclideanToKeypoint( - Eigen::Vector3d p) { - keypoint_t kp; - _camera->euclideanToKeypoint(p, kp); - return kp; -} - -template -Eigen::VectorXd CameraDesignVariable::homogeneousToKeypoint( - Eigen::Vector4d ph) { - keypoint_t kp; - _camera->homogeneousToKeypoint(ph, kp); - return kp; -} - -template -void CameraDesignVariable::evaluateJacobians( - JacobianContainer & outJacobians, Eigen::Vector4d ph) const { - - Eigen::MatrixXd Jp; - Eigen::MatrixXd Jd; - Eigen::MatrixXd Js; - - if (_projectionDv->isActive()) { - _camera->homogeneousToKeypointIntrinsicsJacobian(ph, Jp); - outJacobians.add(_projectionDv.get(), -Jp); - } - if (_distortionDv->isActive()) { - _camera->homogeneousToKeypointDistortionJacobian(ph, Jd); - outJacobians.add(_distortionDv.get(), -Jd); - } - if (_shutterDv->isActive()) { - _camera->homogeneousToKeypointShutterJacobian(ph, Js); - outJacobians.add(_shutterDv.get(), -Js); - } - -} - -template -void CameraDesignVariable::evaluateJacobians( - JacobianContainer & outJacobians, const Eigen::MatrixXd & applyChainRule, - Eigen::Vector4d ph) const { - - Eigen::MatrixXd Jp; - Eigen::MatrixXd Jd; - Eigen::MatrixXd Js; - - _camera->homogeneousToKeypointIntrinsicsJacobian(ph, Jp); - _camera->homogeneousToKeypointDistortionJacobian(ph, Jd); - _camera->homogeneousToKeypointShutterJacobian(ph, Js); - - outJacobians.add(_projectionDv.get(), -applyChainRule * Jp); - outJacobians.add(_distortionDv.get(), -applyChainRule * Jd); - outJacobians.add(_shutterDv.get(), -applyChainRule * Js); - -} - -template -void CameraDesignVariable::getDesignVariables( - DesignVariable::set_t & designVariables) const { - - // add the 3 camera DV: distortion, intrinsics, shutter - designVariables.insert(_projectionDv.get()); - designVariables.insert(_distortionDv.get()); - designVariables.insert(_shutterDv.get()); - -} - -} // backend -} // aslam diff --git a/aslam_cv/aslam_cv_backend_python/package.xml b/aslam_cv/aslam_cv_backend_python/package.xml index 412e67f56..9b6df1b7a 100644 --- a/aslam_cv/aslam_cv_backend_python/package.xml +++ b/aslam_cv/aslam_cv_backend_python/package.xml @@ -8,7 +8,7 @@ New BSD catkin catkin_simple - + sm_common sm_python sm_opencv @@ -27,6 +27,7 @@ aslam_camera_system aslam_frames aslam_cv_error_terms + aslam_splines aslam_cv_python aslam_cv_backend aslam_time diff --git a/aslam_cv/aslam_cv_backend_python/python/aslam_cv_backend/__init__.py b/aslam_cv/aslam_cv_backend_python/python/aslam_cv_backend/__init__.py index efc1fa429..73e3b08fe 100644 --- a/aslam_cv/aslam_cv_backend_python/python/aslam_cv_backend/__init__.py +++ b/aslam_cv/aslam_cv_backend_python/python/aslam_cv_backend/__init__.py @@ -28,6 +28,7 @@ class DistortedOmniRs(CameraModel): geometry = aslam_cv.DistortedOmniRsCameraGeometry reprojectionError = DistortedOmniRsReprojectionError reprojectionErrorSimple = DistortedOmniRsReprojectionErrorSimple + reprojectionErrorAdaptiveCovariance = DistortedOmniRsReprojectionErrorAdaptiveCovariance designVariable = DistortedOmniRsCameraGeometryDesignVariable projectionType = aslam_cv.DistortedOmniProjection distortionType = aslam_cv.RadialTangentialDistortion @@ -47,10 +48,12 @@ class DistortedPinholeRs(CameraModel): geometry = aslam_cv.DistortedPinholeRsCameraGeometry reprojectionError = DistortedPinholeRsReprojectionError reprojectionErrorSimple = DistortedPinholeRsReprojectionErrorSimple + reprojectionErrorAdaptiveCovariance = DistortedPinholeRsReprojectionErrorAdaptiveCovariance designVariable = DistortedPinholeRsCameraGeometryDesignVariable projectionType = aslam_cv.DistortedPinholeProjection distortionType = aslam_cv.RadialTangentialDistortion shutterType = aslam_cv.RollingShutter + frameType = aslam_cv.DistortedPinholeRsFrame class EquidistantPinhole(CameraModel): geometry = aslam_cv.EquidistantDistortedPinholeCameraGeometry @@ -61,11 +64,12 @@ class EquidistantPinhole(CameraModel): distortionType = aslam_cv.EquidistantDistortion shutterType = aslam_cv.GlobalShutter frameType = aslam_cv.EquidistantDistortedPinholeFrame - + class EquidistantPinholeRs(CameraModel): geometry = aslam_cv.EquidistantDistortedPinholeRsCameraGeometry reprojectionError = EquidistantDistortedPinholeRsReprojectionError reprojectionErrorSimple = EquidistantDistortedPinholeRsReprojectionErrorSimple + reprojectionErrorAdaptiveCovariance = EquidistantDistortedPinholeRsReprojectionErrorAdaptiveCovariance designVariable = EquidistantDistortedPinholeRsCameraGeometryDesignVariable projectionType = aslam_cv.EquidistantPinholeProjection distortionType = aslam_cv.EquidistantDistortion @@ -80,4 +84,3 @@ class FovPinhole(CameraModel): distortionType = aslam_cv.FovDistortion shutterType = aslam_cv.GlobalShutter frameType = aslam_cv.FovDistortedPinholeFrame - \ No newline at end of file diff --git a/aslam_cv/aslam_cv_backend_python/src/module.cpp b/aslam_cv/aslam_cv_backend_python/src/module.cpp index b46a59710..31b5542c8 100644 --- a/aslam_cv/aslam_cv_backend_python/src/module.cpp +++ b/aslam_cv/aslam_cv_backend_python/src/module.cpp @@ -12,38 +12,48 @@ #include void exportGridCalibration(); -void exportCameraGeometryDvc(); +// void exportCameraGeometryDvc(); //void exportNCameraSystemDvc(); BOOST_PYTHON_MODULE(libaslam_cv_backend_python) { exportGridCalibration(); - exportCameraGeometryDvc(); -//exportNCameraSystemDvc(); + // exportCameraGeometryDvc(); + //exportNCameraSystemDvc(); + using namespace aslam::cameras; aslam::python::exportReprojectionErrors("Pinhole"); aslam::python::exportReprojectionErrors("PinholeRs"); + aslam::python::exportCovarianceReprojectionError("PinholeRs"); aslam::python::exportReprojectionErrors("DistortedPinhole"); aslam::python::exportReprojectionErrors("DistortedPinholeRs"); + aslam::python::exportCovarianceReprojectionError("DistortedPinholeRs"); aslam::python::exportReprojectionErrors("EquidistantDistortedPinhole"); aslam::python::exportReprojectionErrors("EquidistantDistortedPinholeRs"); + aslam::python::exportCovarianceReprojectionError("EquidistantDistortedPinholeRs"); aslam::python::exportReprojectionErrors("FovDistortedPinhole"); aslam::python::exportReprojectionErrors("FovDistortedPinholeRs"); + aslam::python::exportCovarianceReprojectionError("FovDistortedPinholeRs"); aslam::python::exportReprojectionErrors("Omni"); aslam::python::exportReprojectionErrors("OmniRs"); + aslam::python::exportCovarianceReprojectionError("OmniRs"); aslam::python::exportReprojectionErrors("DistortedOmni"); aslam::python::exportReprojectionErrors("DistortedOmniRs"); + aslam::python::exportCovarianceReprojectionError("DistortedOmniRs"); aslam::python::exportReprojectionErrors("EquidistantDistortedOmni"); aslam::python::exportReprojectionErrors("EquidistantDistortedOmniRs"); + aslam::python::exportCovarianceReprojectionError("EquidistantDistortedOmniRs"); aslam::python::exportReprojectionErrors("FovDistortedOmni"); aslam::python::exportReprojectionErrors("FovDistortedOmniRs"); + aslam::python::exportCovarianceReprojectionError("FovDistortedOmniRs"); + // Export the camera design variables: using namespace aslam::python; diff --git a/aslam_nonparametric_estimation/aslam_splines/include/aslam/backend/CovarianceReprojectionError.hpp b/aslam_cv/aslam_cv_error_terms/include/aslam/backend/CovarianceReprojectionError.hpp similarity index 75% rename from aslam_nonparametric_estimation/aslam_splines/include/aslam/backend/CovarianceReprojectionError.hpp rename to aslam_cv/aslam_cv_error_terms/include/aslam/backend/CovarianceReprojectionError.hpp index a9e63daa7..fbf7b1767 100644 --- a/aslam_nonparametric_estimation/aslam_splines/include/aslam/backend/CovarianceReprojectionError.hpp +++ b/aslam_cv/aslam_cv_error_terms/include/aslam/backend/CovarianceReprojectionError.hpp @@ -12,12 +12,12 @@ namespace aslam { namespace backend { - + template class CovarianceReprojectionError : public ErrorTermFs< FRAME_T::KeypointDimension > { public: - EIGEN_MAKE_ALIGNED_OPERATOR_NEW + EIGEN_MAKE_ALIGNED_OPERATOR_NEW SM_DEFINE_EXCEPTION(Exception, std::runtime_error); @@ -35,28 +35,29 @@ namespace aslam { typedef ErrorTermFs< KeypointDimension > parent_t; CovarianceReprojectionError(); - // we take the lineDelayDv scalar design variable as an additional parameter as it was - // the structure in the first place and the Jacobians of the Shutters are not yet - // implemented. This should be removed at some point, to only keep the camera - // design variable. - // if the lineDelayDv is initialised to NULL, the camera design variable will be used. - CovarianceReprojectionError(const frame_t * frame, int keypointIndex, HomogeneousExpression point, CameraDesignVariable camera, spline_t* spline = NULL, Scalar * lineDelayDv = NULL); + CovarianceReprojectionError( + const frame_t * frame, + int keypointIndex, + const HomogeneousExpression & point, + CameraDesignVariable camera, + spline_t* spline = NULL + ); virtual ~CovarianceReprojectionError(); double observationTime(); - Eigen::MatrixXd covarianceMatrix(); + Eigen::MatrixXd covarianceMap(); protected: /// \brief evaluate the error term virtual double evaluateErrorImplementation(); - + /// \brief evaluate the jacobian virtual void evaluateJacobiansImplementation(JacobianContainer & J); /// \brief the frame that this measurement comes from. const frame_t * _frame; - + /// \brief the keypoint index within the frame. int _keypointIndex; @@ -66,7 +67,6 @@ namespace aslam { CameraDesignVariable _camera; spline_t * _spline; - Scalar * _lineDelayDv; }; } // namespace backend } // namespace aslam diff --git a/aslam_nonparametric_estimation/aslam_splines/include/aslam/backend/implementation/CovarianceReprojectionError.hpp b/aslam_cv/aslam_cv_error_terms/include/aslam/backend/implementation/CovarianceReprojectionError.hpp similarity index 76% rename from aslam_nonparametric_estimation/aslam_splines/include/aslam/backend/implementation/CovarianceReprojectionError.hpp rename to aslam_cv/aslam_cv_error_terms/include/aslam/backend/implementation/CovarianceReprojectionError.hpp index 949131627..4634cfa9c 100644 --- a/aslam_nonparametric_estimation/aslam_splines/include/aslam/backend/implementation/CovarianceReprojectionError.hpp +++ b/aslam_cv/aslam_cv_error_terms/include/aslam/backend/implementation/CovarianceReprojectionError.hpp @@ -10,11 +10,22 @@ namespace aslam { template - CovarianceReprojectionError::CovarianceReprojectionError(const frame_t * frame, int keypointIndex, - HomogeneousExpression point, CameraDesignVariable camera, spline_t* spline, Scalar* lineDelayDv) : - _frame(frame), _keypointIndex(keypointIndex), _point(point), _camera(camera), _spline(spline), _lineDelayDv(lineDelayDv) + CovarianceReprojectionError::CovarianceReprojectionError( + const frame_t * frame, + int keypointIndex, + const HomogeneousExpression & point, + CameraDesignVariable camera, + spline_t* spline + ) : + _frame(frame), + _keypointIndex(keypointIndex), + _point(point), + _camera(camera), + _spline(spline) { SM_ASSERT_TRUE(Exception, frame != NULL, "The frame must not be null"); + SM_ASSERT_TRUE(Exception, _frame->numKeypoints() > _keypointIndex, "Keypoint index must be in bounds of frame."); + // if a spline is given, estimate the covariance in each iteration //if(!spline) // parent_t::_invR = _frame->keypoint(_keypointIndex).invR(); @@ -23,7 +34,6 @@ namespace aslam { camera.getDesignVariables(dvs); // camera dv's parent_t::setDesignVariablesIterator(dvs.begin(), dvs.end()); - } @@ -34,7 +44,7 @@ namespace aslam { } template - Eigen::MatrixXd CovarianceReprojectionError::covarianceMatrix() { + Eigen::MatrixXd CovarianceReprojectionError::covarianceMap() { const keypoint_t & k = _frame->keypoint(_keypointIndex); const camera_geometry_t & cam = _frame->geometry(); @@ -44,14 +54,9 @@ namespace aslam { Eigen::Matrix outJp; cam.homogeneousToKeypoint(p, hat_y, outJp); + double lineDelay = cam.shutter().lineDelay(); - double lineDelay; - if(_lineDelayDv) - lineDelay = _lineDelayDv->toScalar(); - else - lineDelay = cam.shutter().lineDelay(); - - double observationTime = _frame->keypointTime(_keypointIndex).toSec() + k.y()(1) * lineDelay; + double observationTime = this->observationTime(); Eigen::VectorXd splinePoint = _spline->spline().evalD(observationTime, 0); // evaluate the covariance: @@ -70,38 +75,37 @@ namespace aslam { A(0,1) += J(0); A(1,1) += J(1); - + // make sure, the variance of v remains positive and set to 0 if it is negative - + // if (A(1,1) < 0) { // std::cout << "set one of them to zero..." << std::endl; // A(1,1) = 0; ======> THIS IS BAD! // } - + return A; } - template double CovarianceReprojectionError::evaluateErrorImplementation() { const keypoint_t & k = _frame->keypoint(_keypointIndex); const camera_geometry_t & cam = _frame->geometry(); - + Eigen::Vector4d p = _point.toHomogeneous(); measurement_t hat_y; Eigen::Matrix outJp; - cam.homogeneousToKeypoint(p, hat_y, outJp); + cam.homogeneousToKeypoint(p, hat_y, outJp); parent_t::setError(k.y() - hat_y); if(_spline) { - Eigen::MatrixXd A = covarianceMatrix(); + Eigen::MatrixXd A = covarianceMap(); //std::cout << A << std::endl; - parent_t::setInvR ( ((A*_frame->keypoint(_keypointIndex).invR().inverse()*A.transpose()).inverse())); // invert + parent_t::setInvR (((A*_frame->keypoint(_keypointIndex).invR().inverse()*A.transpose()).inverse())); // invert // setInvR(A*_frame->keypoint(_keypointIndex).invR()*A.transpose()); // std::cout << "A: " << std::endl << A << std::endl ; @@ -114,31 +118,24 @@ namespace aslam { template double CovarianceReprojectionError::observationTime() { - double lineDelay; - if(_lineDelayDv) - lineDelay = _lineDelayDv->toScalar(); - else - lineDelay = _frame->geometry().shutter().lineDelay(); - return _frame->keypointTime(_keypointIndex).toSec() + _frame->keypoint(_keypointIndex).y()(1) * lineDelay; + return _frame->keypointTime(_keypointIndex).toSec(); // + _frame->keypoint(_keypointIndex).y()(1) * lineDelay; } - template void CovarianceReprojectionError::evaluateJacobiansImplementation(aslam::backend::JacobianContainer & _jacobians) { //const keypoint_t & k = _frame->keypoint(_keypointIndex); const camera_geometry_t & cam = _frame->geometry(); - + Eigen::Vector4d p = _point.toHomogeneous(); typename camera_geometry_t::jacobian_homogeneous_t J; measurement_t hat_y; - cam.homogeneousToKeypoint(p, hat_y, J); + cam.homogeneousToKeypoint(p, hat_y, J); _point.evaluateJacobians(_jacobians, -J); _camera.evaluateJacobians(_jacobians, p); - } diff --git a/aslam_cv/aslam_cv_error_terms/package.xml b/aslam_cv/aslam_cv_error_terms/package.xml index deaf8b420..aaecfb4d3 100644 --- a/aslam_cv/aslam_cv_error_terms/package.xml +++ b/aslam_cv/aslam_cv_error_terms/package.xml @@ -16,6 +16,7 @@ aslam_time aslam_cameras aslam_backend + aslam_cv_backend aslam_backend_expressions sm_boost sm_eigen @@ -23,4 +24,5 @@ sparse_block_matrix suitesparse opencv2_catkin + aslam_splines diff --git a/aslam_cv/aslam_cv_python/src/CameraProjections.cpp b/aslam_cv/aslam_cv_python/src/CameraProjections.cpp index 9aa399827..4d3273e9a 100644 --- a/aslam_cv/aslam_cv_python/src/CameraProjections.cpp +++ b/aslam_cv/aslam_cv_python/src/CameraProjections.cpp @@ -215,6 +215,8 @@ void exportGenericDistortionFunctions(T & dist) { void exportFovDistortionFunctions() { class_ > distortion("FovDistortion", init<>()); + register_ptr_to_python >(); + exportGenericDistortionFunctions(distortion); distortion.def(init(("FovDistortion(double w)"))); distortion.def("w", &FovDistortion::w); @@ -225,6 +227,8 @@ void exportRadialTangentialDistortionFunctions() { class_ > rtDistortion( "RadialTangentialDistortion", init<>()); + register_ptr_to_python >(); + exportGenericDistortionFunctions(rtDistortion); rtDistortion.def( @@ -247,6 +251,8 @@ void exportOmniProjection(std::string name) { class_, boost::shared_ptr > > omniProjection( name.c_str(), init<>()); + register_ptr_to_python > >(); + omniProjection.def(init<>((name + "(distortion_t distortion)").c_str())).def( init( (name @@ -289,6 +295,8 @@ void exportPinholeProjection(std::string name) { class_, boost::shared_ptr > > pinholeProjection( name.c_str(), init<>()); + register_ptr_to_python > >(); + pinholeProjection.def(init<>((name + "(distortion_t distortion)").c_str())) .def( init( @@ -331,10 +339,14 @@ void exportCameraProjections() { class_ > noDistortion( "NoDistortion", init<>()); + register_ptr_to_python >(); + exportGenericDistortionFunctions(noDistortion); class_ > equidistantDistortion( "EquidistantDistortion", init<>()); + register_ptr_to_python >(); + equidistantDistortion.def(init()); exportGenericDistortionFunctions( equidistantDistortion); diff --git a/aslam_cv/aslam_cv_python/src/CameraShutters.cpp b/aslam_cv/aslam_cv_python/src/CameraShutters.cpp index dad33232f..74518f303 100644 --- a/aslam_cv/aslam_cv_python/src/CameraShutters.cpp +++ b/aslam_cv/aslam_cv_python/src/CameraShutters.cpp @@ -40,6 +40,8 @@ void exportGlobalShutter(std::string name) { name.c_str(), init<>()); globalShutter.def(init<>((name + "()").c_str())); + register_ptr_to_python >(); + exportGenericShutterFunctions(globalShutter); } @@ -52,6 +54,8 @@ void exportRollingShutter(std::string name) { "lineDelay", &RollingShutter::lineDelay, "Returns the Line Delay of the Rolling Shutter"); + register_ptr_to_python >(); + exportGenericShutterFunctions(rollingShutter); } diff --git a/aslam_cv/aslam_cv_serialization/CMakeLists.txt b/aslam_cv/aslam_cv_serialization/CMakeLists.txt index ffafe88f0..f6befc888 100644 --- a/aslam_cv/aslam_cv_serialization/CMakeLists.txt +++ b/aslam_cv/aslam_cv_serialization/CMakeLists.txt @@ -1,3 +1,4 @@ + cmake_minimum_required(VERSION 2.8.3) project(aslam_cv_serialization) @@ -8,13 +9,13 @@ catkin_simple() INCLUDE(autogen_cameras.cmake) #INCLUDE(autogen_frames.cmake) -cs_add_library(${PROJECT_NAME} +cs_add_library(${PROJECT_NAME} # ${AUTOGEN_FRAME_CPP_FILES} ${AUTOGEN_CAMERA_CPP_FILES} src/FrameBaseSerialization.cpp ) -find_package(Boost REQUIRED COMPONENTS serialization) +find_package(Boost REQUIRED COMPONENTS serialization system) target_link_libraries(${PROJECT_NAME} ${Boost_LIBRARIES}) # Avoid clash with tr1::tuple: https://code.google.com/p/googletest/source/browse/trunk/README?r=589#257 diff --git a/aslam_cv/aslam_cv_serialization/package.xml b/aslam_cv/aslam_cv_serialization/package.xml index c65cd8102..d56c81e1e 100644 --- a/aslam_cv/aslam_cv_serialization/package.xml +++ b/aslam_cv/aslam_cv_serialization/package.xml @@ -22,6 +22,7 @@ sm_opencv sm_logging opencv2_catkin + sm_random eigen diff --git a/aslam_incremental_calibration/incremental_calibration/test/IncrementalOptimizationProblemTest.cpp b/aslam_incremental_calibration/incremental_calibration/test/IncrementalOptimizationProblemTest.cpp index 9128ca48a..4f2225e6f 100644 --- a/aslam_incremental_calibration/incremental_calibration/test/IncrementalOptimizationProblemTest.cpp +++ b/aslam_incremental_calibration/incremental_calibration/test/IncrementalOptimizationProblemTest.cpp @@ -31,6 +31,7 @@ #include "aslam/calibration/core/IncrementalOptimizationProblem.h" #include "aslam/calibration/core/OptimizationProblem.h" #include "aslam/calibration/data-structures/VectorDesignVariable.h" +#include #include "aslam/calibration/exceptions/OutOfBoundException.h" #include "aslam/calibration/exceptions/InvalidOperationException.h" @@ -158,7 +159,7 @@ TEST(AslamCalibrationTestSuite, testIncrementalOptimizationProblem) { ASSERT_EQ(incProblem.errorTerm(6), et7.get()); ASSERT_EQ(incProblem.errorTerm(7), et8.get()); ASSERT_EQ(incProblem.errorTerm(8), et9.get()); - ASSERT_THROW(incProblem.errorTerm(9), OutOfBoundException); + ASSERT_THROW(incProblem.errorTerm(9), aslam::Exception); ASSERT_EQ(incProblem.getNumGroups(), 2); ASSERT_EQ(incProblem.getGroupId(dv1.get()), 0); ASSERT_EQ(incProblem.getGroupId(dv2.get()), 0); @@ -181,7 +182,7 @@ TEST(AslamCalibrationTestSuite, testIncrementalOptimizationProblem) { ASSERT_EQ(incProblem.designVariable(3), dv5.get()); ASSERT_EQ(incProblem.designVariable(4), dv6.get()); ASSERT_EQ(incProblem.designVariable(5), dv3.get()); - ASSERT_THROW(incProblem.designVariable(7), OutOfBoundException); + ASSERT_THROW(incProblem.designVariable(7), aslam::Exception); incProblem.setGroupsOrdering({1, 0}); ASSERT_EQ(incProblem.getGroupsOrdering(), std::vector({1, 0})); ASSERT_EQ(incProblem.designVariable(0), dv3.get()); @@ -213,7 +214,7 @@ TEST(AslamCalibrationTestSuite, testIncrementalOptimizationProblem) { ASSERT_EQ(incProblem.errorTerm(4), et5.get()); ASSERT_EQ(incProblem.errorTerm(5), et6.get()); ASSERT_EQ(incProblem.errorTerm(6), et7.get()); - ASSERT_THROW(incProblem.errorTerm(7), OutOfBoundException); + ASSERT_THROW(incProblem.errorTerm(7), aslam::Exception); Eigen::MatrixXd dv1Param; Eigen::MatrixXd dv6Param; incProblem.saveDesignVariables(); diff --git a/aslam_incremental_calibration/incremental_calibration/test/OptimizationProblemTest.cpp b/aslam_incremental_calibration/incremental_calibration/test/OptimizationProblemTest.cpp index c6f21495f..9a375ebaa 100644 --- a/aslam_incremental_calibration/incremental_calibration/test/OptimizationProblemTest.cpp +++ b/aslam_incremental_calibration/incremental_calibration/test/OptimizationProblemTest.cpp @@ -30,6 +30,7 @@ #include "aslam/calibration/core/OptimizationProblem.h" #include "aslam/calibration/data-structures/VectorDesignVariable.h" +#include #include "aslam/calibration/exceptions/OutOfBoundException.h" #include "aslam/calibration/exceptions/InvalidOperationException.h" @@ -95,7 +96,7 @@ TEST(AslamCalibrationTestSuite, testOptimizationProblem) { ASSERT_EQ(problem.designVariable(0), dv2.get()); ASSERT_EQ(problem.designVariable(1), dv3.get()); ASSERT_EQ(problem.designVariable(2), dv1.get()); - ASSERT_THROW(problem.designVariable(3), OutOfBoundException); + ASSERT_THROW(problem.designVariable(3), aslam::Exception); problem.permuteDesignVariables({1, 0}, 1); ASSERT_EQ(problem.designVariable(0), dv3.get()); ASSERT_THROW(problem.permuteDesignVariables({1, 0}, 2), @@ -116,7 +117,7 @@ TEST(AslamCalibrationTestSuite, testOptimizationProblem) { ASSERT_EQ(problem.errorTerm(0), et1.get()); ASSERT_EQ(problem.errorTerm(1), et2.get()); ASSERT_EQ(problem.errorTerm(2), et3.get()); - ASSERT_THROW(problem.errorTerm(3), OutOfBoundException); + ASSERT_THROW(problem.errorTerm(3), aslam::Exception); ASSERT_THROW(problem.addErrorTerm(et1), InvalidOperationException); problem.permuteErrorTerms({2, 1, 0}); auto etsp = problem.getErrorTerms(); diff --git a/aslam_offline_calibration/kalibr/python/kalibr_calibrate_rs_cameras b/aslam_offline_calibration/kalibr/python/kalibr_calibrate_rs_cameras new file mode 100644 index 000000000..b4d2bf1f6 --- /dev/null +++ b/aslam_offline_calibration/kalibr/python/kalibr_calibrate_rs_cameras @@ -0,0 +1,181 @@ +#!/usr/bin/env python +print "importing libraries" +import rosbag +import cv_bridge + +import sm +from sm import PlotCollection +import aslam_cv as acv +import aslam_cameras_april as acv_april +import aslam_cv_backend as acvb +import kalibr_common as kc +import kalibr_camera_calibration as kcc +import kalibr_rs_camera_calibration as rscc + +import cv +import cv2 +import os +import numpy as np +import pylab as pl +import argparse +import sys +import random +import signal + +np.set_printoptions(suppress=True) + +def __initBagDataset(bagfile, topic, from_to): + print "\tDataset: {0}".format(bagfile) + print "\tTopic: {0}".format(topic) + reader = kc.BagImageDatasetReader(bagfile, topic, bag_from_to=from_to) + print "\tNumber of images: {0}".format(reader.numImages()) + return reader + +#available models +cameraModels = { + # rolling shutter + 'pinhole-radtan-rs': acvb.DistortedPinholeRs, + 'pinhole-equi-rs': acvb.EquidistantPinholeRs, + 'omni-radtan-rs': acvb.DistortedOmniRs, + # global shutter + 'pinhole-radtan': acvb.DistortedPinhole, + 'pinhole-equi': acvb.EquidistantPinhole, + 'omni-radtan': acvb.DistortedOmni +} + +def __signal_exit(signal, frame): + sm.logWarn("Shutdown requested! (CTRL+C)") + sys.exit(2) + +def __parseArgs(): + class KalibrArgParser(argparse.ArgumentParser): + def error(self, message): + self.print_help() + sm.logError('%s' % message) + sys.exit(2) + def format_help(self): + formatter = self._get_formatter() + formatter.add_text(self.description) + formatter.add_usage(self.usage, self._actions, + self._mutually_exclusive_groups) + for action_group in self._action_groups: + formatter.start_section(action_group.title) + formatter.add_text(action_group.description) + formatter.add_arguments(action_group._group_actions) + formatter.end_section() + formatter.add_text(self.epilog) + return formatter.format_help() + + usage = """ + Example usage to calibrate a camera system with a single rolling shutter camera using an aprilgrid. + + cam: pinhole model with radial-tangential distortion and rolling shutter + + %(prog)s --models pinhole-radtan-rs --target aprilgrid.yaml \\ + --bag MYROSBAG.bag --topics /cam0/image_raw /cam1/image_raw + + example aprilgrid.yaml: + target_type: 'aprilgrid' + tagCols: 6 + tagRows: 6 + tagSize: 0.088 #m + tagSpacing: 0.3 #percent of tagSize""" + + parser = KalibrArgParser(description='Calibrate the intrinsics and extrinsics of a rolling shutter camera.', usage=usage) + parser.add_argument('--model', dest='model', help='The camera model to estimate'.format(cameraModels.keys()), required=True) + parser.add_argument('--framerate', dest='framerate', type=int, help='Approximate framerate of the camera', required=True) + + groupSource = parser.add_argument_group('Data source') + groupSource.add_argument('--bag', dest='bagfile', help='The bag file with the data') + groupSource.add_argument('--topic', dest='topic', help='The image topic', required=True) + groupSource.add_argument('--bag-from-to', metavar='bag_from_to', type=float, nargs=2, help='Use the bag data starting from up to this time [s]') + + groupTarget = parser.add_argument_group('Calibration target configuration') + groupTarget.add_argument('--target', dest='targetYaml', help='Calibration target configuration as yaml file', required=True) + groupTarget.add_argument('--feature-variance', dest='inverseFeatureVariance', type=float, help='Variance of the feature detector', required=True) + + groupOpt = parser.add_argument_group('Optimization options') + groupOpt.add_argument('--max-iter', type=int, default=30, dest='max_iter', help='Max. iterations (default: %(default)s)', required=False) + + outputSettings = parser.add_argument_group('Output options') + outputSettings.add_argument('--verbose', action='store_true', dest='verbose', help='Enable (really) verbose output (disables plots)') + outputSettings.add_argument('--show-extraction', action='store_true', dest='showextraction', help='Show the calibration target extraction. (disables plots)') + + # print help if no argument is specified + if len(sys.argv)==1: + parser.print_help() + sys.exit(2) + + # Parse the argument list + try: + parsed = parser.parse_args() + except: + sys.exit(2) + + # there is an issue with the gtk plot widget, so we cant plot if we have opencv windows open... + # --> disable the plots in these special situations + if parsed.showextraction or parsed.verbose: + parsed.dontShowReport = True + + return parsed + +# perform corner extraction from bagfile +# returns the observations +def __extractObservations(cameraGeometry, verbose=False): + + # extract the targets + multithreading = False #not verbose + observations = kc.extractCornersFromDataset( + cameraGeometry.dataset, + cameraGeometry.ctarget.detector, + multithreading=multithreading, + # transformation estimation will fail with rs cameras and significant distortions + noTransformation=True + ) + + return observations + +def main(): + parsed = __parseArgs() + + # logging modes + if parsed.verbose: + sm.setLoggingLevel(sm.LoggingLevel.Debug) + else: + sm.setLoggingLevel(sm.LoggingLevel.Info) + + # register signal handler + signal.signal(signal.SIGINT, __signal_exit) + + ###### + ## load bagfile and extract targets: + targetConfig = kc.CalibrationTargetParameters(parsed.targetYaml) + dataset = __initBagDataset(parsed.bagfile, parsed.topic, parsed.bag_from_to) + + #create camera + cameraModel = cameraModels[parsed.model] + cameraGeometry = kcc.CameraGeometry(cameraModel, targetConfig, dataset, verbose=(parsed.verbose or parsed.showextraction)) + + # extract observations + observations = __extractObservations(cameraGeometry,(parsed.verbose or parsed.showextraction)) + + # Calibration Configuration + config = rscc.RsCalibratorConfiguration() + config.inverseFeatureCovariance = 1.0/parsed.inverseFeatureVariance + config.maxNumberOfIterations = parsed.max_iter + config.framerate = parsed.framerate + config.timeOffsetConstantSparsityPattern = 0.08 + config.timeOffsetPadding = 0.5 + + ###### + ## Calibrate + calibrator = rscc.RsCalibrator() + calibrator.calibrate(cameraGeometry, observations, config) + +if __name__ == "__main__": + try: + main() + except Exception,e: + sm.logError("Exception: {0}".format(e)) + sys.exit(-1) + diff --git a/aslam_offline_calibration/kalibr/python/kalibr_rs_camera_calibration/ReprojectionErrorKnotSequenceUpdateStrategy.py b/aslam_offline_calibration/kalibr/python/kalibr_rs_camera_calibration/ReprojectionErrorKnotSequenceUpdateStrategy.py new file mode 100644 index 000000000..4180ba131 --- /dev/null +++ b/aslam_offline_calibration/kalibr/python/kalibr_rs_camera_calibration/ReprojectionErrorKnotSequenceUpdateStrategy.py @@ -0,0 +1,190 @@ +#encoding:UTF-8 +import numpy as np +import bsplines + +# Knot Sequence Update Strategies need two method: +# 1) generateKnotList: which generates a new knotlist given the current spline and reprojection errors +# Returns a boolean flag if the updated knot sequence needs another step of optimization +# 2) getUpdatedSpline: given a knot list, spline order and spline generates a new spline initialized with the +# values of the given spline and the given knot sequence. +class ReprojectionErrorKnotSequenceUpdateStrategy(object): + + __previousKnotSequence = None + + __previousErrorTerms = None + + __framerate = None + + __maxKnotsPerSecond = None + + __disabledTimeSegments = [] + + def __init__(self, framerate): + self.__framerate = framerate + self.__maxKnotsPerSecond = 1/(2*self.__framerate) + + def generateKnotList(self, reprojection_errors, poseSpline): + [times, errors] = self.__getErrorAndTimestamp(reprojection_errors) + + # take a copy of the old knots: + knots = poseSpline.knots() + + [errorTermsPerSegment, errorPerSegment] = self.__getErrorPerSegment(times, errors, knots) + + disabledTimeSegments = self.__removeSegmentsWithoutImprovement(times, errors, self.__disabledTimeSegments) + + [filteredKnots, disabledTimeSegments] = self.__removeSegmentsWithoutObservations(knots, errorPerSegment, disabledTimeSegments) + + [errorTermsPerSegmentFiltered, errorPerSegmentFiltered] = self.__getErrorPerSegment(times, errors, filteredKnots) + + updatedKnots = self.__generateKnotSequence(errorPerSegmentFiltered, errorTermsPerSegmentFiltered, knots, disabledTimeSegments) + + if (self.__previousKnotSequence is None): + requiresUpdate = True + else: + # require at least a 1% increase in knots for a next iteration being worth the effort + requiresUpdate = (len(updatedKnots) > 1.01*len(self.__previousKnotSequence)) + + # keep a copy of the knot sequence + self.__previousKnotSequence = np.copy(updatedKnots) + self.__previousErrorTerms = errorPerSegmentFiltered + self.__disabledTimeSegments = disabledTimeSegments + + return [updatedKnots, requiresUpdate] + + def getUpdatedSpline(self, poseSpline, knots, splineOrder): + """Get a spline with the new knot sequence build upon the poses of the old spline""" + + # linearly sample the old spline + times = np.linspace(poseSpline.t_min(), poseSpline.t_max(), len(knots)) + + splinePoses = np.zeros((6, len(knots))) + for i, time in enumerate(times): + splinePoses[:,i] = poseSpline.eval(time) + + # guarantee that beginning and end times of the spline remain unchanged + oldKnots = poseSpline.knots() + + i = 0 + while oldKnots[i] < knots[0]: + i += 1 + knots = np.insert(knots, 0, oldKnots[0:i]) + + i = -1 + while oldKnots[i] > knots[-1]: + i -= 1 + knots = np.append(knots, oldKnots[i:]) + + newPoseSpline = bsplines.BSplinePose(splineOrder, poseSpline.rotation()) + newPoseSpline.initPoseSplineSparseKnots(times, splinePoses, np.array(knots), 1e-6) + + return newPoseSpline + + + def __getErrorAndTimestamp(self,reprojection_errors): + """Extract the timestamps and reprojection error values""" + errors = [] + times = [] + for reprojection_error in reprojection_errors: + times.append(reprojection_error.observationTime()) + errors.append(reprojection_error.evaluateError()) + + # it is not guaranteed that the errors are sorted in tiem + newIdx = sorted(range(len(times)),key=times.__getitem__) + times = [ times[i] for i in newIdx] + errors = [ errors[i] for i in newIdx] + + return [times, errors] + + def __getErrorPerSegment(self,times, errors, knots): + """Get the total error per segment and number of error terms per segment""" + errorPerSegment = np.zeros(len(knots)) + errorTermsPerSegment = np.zeros(len(knots)) + segment = (-1,-1) + # analyse each section of the knot sequence: + for i, t in enumerate(times): + segment = self.__time2KnotSection(t, knots, segment) + errorPerSegment[segment[0]] += errors[i] + errorTermsPerSegment[segment[0]] += 1 + + return [errorTermsPerSegment, errorPerSegment] + + def __removeSegmentsWithoutObservations(self, knots, errorPerSegment, disabledTimeSegments = []): + filteredKnots = [] + + # remove segments with consecutive 0-valued errors + p_error = 0 + for i, error in enumerate(errorPerSegment): + if p_error == 0 and error == 0 and i > 6 and i < len(errorPerSegment) - 6: # this should depend on the splineOrder! + # add the segment between the previous and the next knot the the "blacklist" + disabledTimeSegments.append((knots[i-1],knots[i+1])) + else: + filteredKnots.append(knots[i]) + p_error = error + + return [filteredKnots, disabledTimeSegments] + + def __generateKnotSequence(self, errorPerSegmentFiltered, errorTermsPerSegmentFiltered, knots, disabledTimeSegments): + newKnots = [] + numberOfKnots = len(knots) + for i, error in enumerate(errorPerSegmentFiltered): + expectedNormalError = errorTermsPerSegmentFiltered[i] + if error > expectedNormalError and i < numberOfKnots-1: + newKnot = (knots[i] + knots[i+1])/2.0 + deltaT = newKnot - knots[i] + # max number of knots per second hit: do not split + if deltaT <= self.__maxKnotsPerSecond: + newKnots.append(knots[i]) + # segment is disabled: do not split + elif (disabledTimeSegments is not None and self.__isSegmentDisabled(disabledTimeSegments, newKnot)): + newKnots.append(knots[i]) + else: + # split: + newKnots.append(knots[i]) + newKnots.append(newKnot) + else: + newKnots.append(knots[i]) + + return newKnots + + def __time2KnotSection(self, t, knots, segment): + i = segment[0] # we assume that the times are ordered thus we do not need to check before the previous segment + if i == -1: + i = 0 + while(i < len(knots)-1): + if knots[i] < t and knots[i+1] > t: + return (i, i+1) + i+=1 + return (-1,-1) + + # true: disabled + # false: not disabled + def __isSegmentDisabled(self, disabledTimeSegments, t): + for seg in disabledTimeSegments: + if t < seg[1] and t >= seg[0]: + return True + return False + + def __removeSegmentsWithoutImprovement(self, times, errors, disabledTimeSegments): + + ## first compare the reprojection error in the "previous" segments + # if we do not observe a significant drop. stop adding errors! + if self.__previousKnotSequence is not None and self.__previousErrorTerms is not None: + timeSegments = [] + errorPerOldSegment = np.zeros(len(self.__previousKnotSequence)) + segment = (-1,-1) + # analyse each section of the knot sequence: + # this kind of inefficient as it adds the same time segment multiple times... + for i, t in enumerate(times): + segment = self.__time2KnotSection(t, self.__previousKnotSequence, segment) + errorPerOldSegment[segment[0]] += errors[i] + timeSegments.append((self.__previousKnotSequence[segment[0]], self.__previousKnotSequence[segment[1]])) + + # disabledTimeSegments = [] + + for i, pe in enumerate(self.__previousErrorTerms): + if pe * 0.8 < errorPerOldSegment[i]: + disabledTimeSegments.append(timeSegments[i]) + pass + + return disabledTimeSegments diff --git a/aslam_offline_calibration/kalibr/python/kalibr_rs_camera_calibration/RsCalibrator.py b/aslam_offline_calibration/kalibr/python/kalibr_rs_camera_calibration/RsCalibrator.py new file mode 100644 index 000000000..ea9a1225d --- /dev/null +++ b/aslam_offline_calibration/kalibr/python/kalibr_rs_camera_calibration/RsCalibrator.py @@ -0,0 +1,456 @@ +#encoding:UTF-8 +import sm +import aslam_backend as aopt +import aslam_cv_backend as acvb +import aslam_cv as acv +import aslam_splines as asp +import incremental_calibration as inc +import bsplines +import numpy as np +import multiprocessing +import sys +import gc +import math +from ReprojectionErrorKnotSequenceUpdateStrategy import * +from RsPlot import plotSpline +from RsPlot import plotSplineValues +import pylab as pl +import pdb + +# make numpy print prettier +np.set_printoptions(suppress=True) + +CALIBRATION_GROUP_ID = 0 + +class RsCalibratorConfiguration(object): + deltaX = 1e-8 + """Stopping criterion for the optimizer on x""" + + deltaJ = 1e-4 + """Stopping criterion for the optimizer on J""" + + maxNumberOfIterations = 20 + """Maximum number of iterations of the batch optimizer""" + + maxKnotPlacementIterations = 10 + """Maximum number of iterations to take in the adaptive knot-placement step""" + + adaptiveKnotPlacement = True + """Whether to enable adaptive knot placement""" + + knotUpdateStrategy = ReprojectionErrorKnotSequenceUpdateStrategy + """The adaptive knot placement strategy to use""" + + timeOffsetConstantSparsityPattern = 0.08 + """A time offset to pad the blocks generated in the hessian/jacobian to ensure a constant symbolic representation + of the batch estimation problem, even when a change in the shutter timing shifts the capture time to another + spline segment. + """ + + inverseFeatureCovariance = 1/0.26 + """The inverse covariance of the feature detector. Used to standardize the error terms.""" + + estimateParameters = {'shutter': True, 'intrinsics': True, 'distortion': True, 'pose': True, 'landmarks': False} + """Which parameters to estimate. Dictionary with shutter, intrinsics, distortion, pose, landmarks as bool""" + + splineOrder = 4 + """Order of the spline to use for ct-parametrization""" + + timeOffsetPadding = 0.05 + """Time offset to add to the beginning and end of the spline to ensure we remain + in-bounds while estimating time-depending parameters that shift the spline. + """ + + numberOfKnots = None + """Set to an integer to start with a fixed number of uniformly distributed knots on the spline.""" + + W = None + """6x6 diagonal matrix with a weak motion prior""" + + framerate = 30 + """The approximate framerate of the camera. Required as approximate threshold in adaptive + knot placement and for initializing a knot sequence if no number of knots is given. + """ + + def validate(self, isRollingShutter): + """Validate the configuration.""" + # only rolling shutters can be estimated + if (not isRollingShutter): + self.estimateParameters['shutter'] = False + self.adaptiveKnotPlacement = False + +class RsCalibrator(object): + + __observations = None + """Store the list of observations""" + + __cameraGeometry = None + """The geometry container of which the calibration is performed.""" + + __camera = None + """The camera geometry itself.""" + + __camera_dv = None + """The camera design variable""" + + __cameraModelFactory = None + """Factory object that can create a typed objects for a camera (error terms, frames, design variables etc)""" + + __poseSpline = None + """The spline describing the pose of the camera""" + + __poseSpline_dv = None + """The design variable representation of the pose spline of the camera""" + + __config = None + """Configuration container \see RsCalibratorConfiguration""" + + __frames = [] + """All frames observed""" + + __reprojection_errors = [] + """Reprojection errors of the latest optimizer iteration""" + + def calibrate(self, + cameraGeometry, + observations, + config + ): + """ + A Motion regularization term is added with low a priori knowledge to avoid + diverging parts in the spline of too many knots are selected/provided or if + no image information is available for long sequences and to regularize the + last few frames (which typically contain no image information but need to have + knots to /close/ the spline). + + Kwargs: + cameraGeometry (kcc.CameraGeometry): a camera geometry object with an initialized target + observations ([]: The list of observation \see extractCornersFromDataset + config (RsCalibratorConfiguration): calibration configuration + """ + + ## set internal objects + self.__observations = observations + self.__cameraGeometry = cameraGeometry + self.__cameraModelFactory = cameraGeometry.model + self.__camera_dv = cameraGeometry.dv + self.__camera = cameraGeometry.geometry + self.__config = config + + self.__config.validate(self.__isRollingShutter()) + + # obtain initial guesses for extrinsics and intrinsics + if (not self.__generateIntrinsicsInitialGuess()): + sm.logError("Could not generate initial guess.") + + # obtain the extrinsic initial guess for every observation + self.__generateExtrinsicsInitialGuess() + + # set the value for the motion prior term or uses the defaults + W = self.__getMotionModelPriorOrDefault() + + self.__poseSpline = self.__generateInitialSpline( + self.__config.splineOrder, + self.__config.timeOffsetPadding, + self.__config.numberOfKnots, + self.__config.framerate + ) + + # build estimator problem + optimisation_problem = self.__buildOptimizationProblem(W) + + self.__runOptimization( + optimisation_problem, + self.__config.deltaJ, + self.__config.deltaX, + self.__config.maxNumberOfIterations + ) + + # continue with knot replacement + if self.__config.adaptiveKnotPlacement: + knotUpdateStrategy = self.__config.knotUpdateStrategy(self.__config.framerate) + + for iteration in range(self.__config.maxKnotPlacementIterations): + + # generate the new knots list + [knots, requiresUpdate] = knotUpdateStrategy.generateKnotList( + self.__reprojection_errors, + self.__poseSpline_dv.spline() + ) + # if no new knotlist was generated, we are done. + if (not requiresUpdate): + break; + + # otherwise update the spline dv and rebuild the problem + self.__poseSpline = knotUpdateStrategy.getUpdatedSpline(self.__poseSpline_dv.spline(), knots, self.__config.splineOrder) + + optimisation_problem = self.__buildOptimizationProblem(W) + self.__runOptimization( + optimisation_problem, + self.__config.deltaJ, + self.__config.deltaX, + self.__config.maxNumberOfIterations + ) + + self.__printResults() + + def __generateExtrinsicsInitialGuess(self): + """Estimate the pose of the camera with a PnP solver. Call after initializing the intrinsics""" + # estimate and set T_c in the observations + for idx, observation in enumerate(self.__observations): + (success, T_t_c) = self.__camera.estimateTransformation(observation) + if (success): + observation.set_T_t_c(T_t_c) + else: + sm.logWarn("Could not estimate T_t_c for observation at index" . idx) + + return + + def __generateIntrinsicsInitialGuess(self): + """ + Get an initial guess for the camera geometry (intrinsics, distortion). Distortion is typically left as 0,0,0,0. + The parameters of the geometryModel are updated in place. + """ + if (self.__isRollingShutter()): + sensorRows = self.__observations[0].imRows() + self.__camera.shutter().setParameters(np.array([1.0 / self.__config.framerate / float(sensorRows)])) + + return self.__camera.initializeIntrinsics(self.__observations) + + def __getMotionModelPriorOrDefault(self): + """Get the motion model prior or the default value""" + W = self.__config.W + if W is None: + W = np.eye(6) + W[:3,:3] *= 1e-3 + W[3:,3:] *= 1 + W *= 1e-2 + return W + + def __generateInitialSpline(self, splineOrder, timeOffsetPadding, numberOfKnots = None, framerate = None): + poseSpline = bsplines.BSplinePose(splineOrder, sm.RotationVector()) + + # Get the observation times. + times = np.array([observation.time().toSec() for observation in self.__observations ]) + # get the pose values of the initial transformations at observation time + curve = np.matrix([ poseSpline.transformationToCurveValue( observation.T_t_c().T() ) for observation in self.__observations]).T + # make sure all values are well defined + if np.isnan(curve).any(): + raise RuntimeError("Nans in curve values") + sys.exit(0) + # Add 2 seconds on either end to allow the spline to slide during optimization + times = np.hstack((times[0] - (timeOffsetPadding * 2.0), times, times[-1] + (timeOffsetPadding * 2.0))) + curve = np.hstack((curve[:,0], curve, curve[:,-1])) + + self.__ensureContinuousRotationVectors(curve) + + seconds = times[-1] - times[0] + + # fixed number of knots + if (numberOfKnots is not None): + knots = numberOfKnots + # otherwise with framerate estimate + else: + knots = int(round(seconds * framerate/3)) + + print + print "Initializing a pose spline with %d knots (%f knots per second over %f seconds)" % ( knots, 100, seconds) + poseSpline.initPoseSplineSparse(times, curve, knots, 1e-4) + + return poseSpline + + def __buildOptimizationProblem(self, W): + """Build the optimisation problem""" + problem = inc.CalibrationOptimizationProblem() + + # Initialize all design variables. + self.__initPoseDesignVariables(problem) + + ##### + ## build error terms and add to problem + + # store all frames + self.__frames = [] + self.__reprojection_errors = [] + + # This code assumes that the order of the landmarks in the observations + # is invariant across all observations. At least for the chessboards it is true. + + ##### + # add all the landmarks once + landmarks = [] + landmarks_expr = [] + for landmark in self.__observations[0].getCornersTargetFrame(): + # design variable for landmark + landmark_w_dv = aopt.HomogeneousPointDv(sm.toHomogeneous(landmark)) + landmark_w_dv.setActive(self.__config.estimateParameters['landmarks']); + landmarks.append(landmark_w_dv) + landmarks_expr.append(landmark_w_dv.toExpression()) + problem.addDesignVariable(landmark_w_dv, CALIBRATION_GROUP_ID) + + ##### + # activate design variables + self.__camera_dv.setActive( + self.__config.estimateParameters['intrinsics'], + self.__config.estimateParameters['distortion'], + self.__config.estimateParameters['shutter'] + ) + + ##### + # Add design variables + + # add the camera design variables last for optimal sparsity patterns + problem.addDesignVariable(self.__camera_dv.shutterDesignVariable(), CALIBRATION_GROUP_ID) + problem.addDesignVariable(self.__camera_dv.projectionDesignVariable(), CALIBRATION_GROUP_ID) + problem.addDesignVariable(self.__camera_dv.distortionDesignVariable(), CALIBRATION_GROUP_ID) + + ##### + # Regularization term / motion prior + motionError = asp.BSplineMotionError(self.__poseSpline_dv, W) + problem.addErrorTerm(motionError) + + ##### + # add a reprojection error for every corner of each observation + for observation in self.__observations: + # only process successful observations of a pattern + if (observation.hasSuccessfulObservation()): + # add a frame + frame = self.__cameraModelFactory.frameType() + frame.setGeometry(self.__camera) + frame.setTime(observation.time()) + self.__frames.append(frame) + + ##### + # add an error term for every observed corner + for index, point in enumerate(observation.getCornersImageFrame()): + # keypoint time offset by line delay as expression type + keypoint_time = self.__camera_dv.keypointTime(frame.time(), point) + + # from target to world transformation. + T_w_t = self.__poseSpline_dv.transformationAtTime( + keypoint_time, + self.__config.timeOffsetConstantSparsityPattern, + self.__config.timeOffsetConstantSparsityPattern + ) + T_t_w = T_w_t.inverse() + + # transform target point to camera frame + p_t = T_t_w * landmarks_expr[index] + + # create the keypoint + keypoint = acv.Keypoint2() + keypoint.setMeasurement(point) + inverseFeatureCovariance = self.__config.inverseFeatureCovariance; + keypoint.setInverseMeasurementCovariance(np.eye(len(point)) * inverseFeatureCovariance) + keypoint.setLandmarkId(index) + frame.addKeypoint(keypoint) + keypoint_index = frame.numKeypoints() - 1 + + # create reprojection error + reprojection_error = self.__buildErrorTerm( + frame, + keypoint_index, + p_t, + self.__camera_dv, + self.__poseSpline_dv + ) + self.__reprojection_errors.append(reprojection_error) + problem.addErrorTerm(reprojection_error) + + return problem + + def __buildErrorTerm(self, frame, keypoint_index, p_t, camera_dv, poseSpline_dv): + """ + Build an error term that considers the shutter type. A Global Shutter camera gets the standard reprojection error + a Rolling Shutter gets the adaptive covariance error term that considers the camera motion. + """ + # it is a global shutter camera -> no covariance error + if (self.__isRollingShutter()): + return self.__cameraModelFactory.reprojectionErrorAdaptiveCovariance( + frame, + keypoint_index, + p_t, + camera_dv, + poseSpline_dv + ) + else: + return self.__cameraModelFactory.reprojectionError( + frame, + keypoint_index, + p_t, + camera_dv + ) + + def __ensureContinuousRotationVectors(self, curve): + """ + Ensures that the rotation vector does not flip and enables a continuous trajectory modeling. + Updates curves in place. + """ + for i in range(1, curve.shape[1]): + previousRotationVector = curve[3:6,i-1] + r = curve[3:6,i] + angle = np.linalg.norm(r) + axis = r/angle + best_r = r + best_dist = np.linalg.norm( best_r - previousRotationVector) + + for s in range(-3,4): + aa = axis * (angle + math.pi * 2.0 * s) + dist = np.linalg.norm( aa - previousRotationVector ) + if dist < best_dist: + best_r = aa + best_dist = dist + curve[3:6,i] = best_r; + + def __initPoseDesignVariables(self, problem): + """Get the design variable representation of the pose spline and add them to the problem""" + # get the design variable + self.__poseSpline_dv = asp.BSplinePoseDesignVariable(self.__poseSpline) + # activate all contained dv and add to problem + for i in range(0, self.__poseSpline_dv.numDesignVariables()): + dv = self.__poseSpline_dv.designVariable(i) + dv.setActive(self.__config.estimateParameters['pose']) + problem.addDesignVariable(dv, CALIBRATION_GROUP_ID) + + def __runOptimization(self, problem ,deltaJ, deltaX, maxIt): + """Run the given optimization problem problem""" + + print "run new optimisation with initial values:" + self.__printResults() + + # verbose and choldmod solving with schur complement trick + options = aopt.Optimizer2Options() + options.verbose = True + options.linearSolver = aopt.BlockCholeskyLinearSystemSolver() + options.doSchurComplement = True + + # stopping criteria + options.maxIterations = maxIt + options.convergenceDeltaJ = deltaJ + options.convergenceDeltaX = deltaX + + # use the dogleg trustregion policy + options.trustRegionPolicy = aopt.DogLegTrustRegionPolicy() + + # create the optimizer + optimizer = aopt.Optimizer2(options) + optimizer.setProblem(problem) + + # go for it: + return optimizer.optimize() + + def __isRollingShutter(self): + return self.__cameraModelFactory.shutterType == acv.RollingShutter + + def __printResults(self): + shutter = self.__camera_dv.shutterDesignVariable().value() + proj = self.__camera_dv.projectionDesignVariable().value() + dist = self.__camera_dv.distortionDesignVariable().value() + print + if (self.__isRollingShutter()): + print "LineDelay:" + print shutter.lineDelay() + print "Intrinsics:" + print "(",proj.fu(),", ",proj.fv(),") (",proj.cu(),", ",proj.cv(),")" + print "Distortion:" + print "(",dist.p1(),", ",dist.p2(),") (",dist.k1(),", ",dist.k2(),")" diff --git a/aslam_offline_calibration/kalibr/python/kalibr_rs_camera_calibration/RsPlot.py b/aslam_offline_calibration/kalibr/python/kalibr_rs_camera_calibration/RsPlot.py new file mode 100644 index 000000000..ce7fccfe1 --- /dev/null +++ b/aslam_offline_calibration/kalibr/python/kalibr_rs_camera_calibration/RsPlot.py @@ -0,0 +1,135 @@ +import sm +import numpy as np +import pylab as pl + +def plotSpline(spline, splineB = None): + """Plots a spline over the full range of times. + + The orientations are shown as EulerAnglesYawPitchRoll. + + Args: + spline (BsplinePose): a pose describing spline + """ + ap = sm.EulerAnglesYawPitchRoll() + t1 = spline.t_min() + t2 = spline.t_max() + times = np.linspace(t1, t2, 1000) + + pos = [] + ori = [] + for t in times: + T = spline.transformation(t) + pos.append(T[:,3]) + ori.append(ap.rotationMatrixToParameters(T[:3,:3])) + + if (splineB is not None): + posB = [] + oriB = [] + for t in times: + T = splineB.transformation(t) + posB.append(T[:,3]) + oriB.append(ap.rotationMatrixToParameters(T[:3,:3])) + posB = np.array(posB) + oriB = np.array(oriB) + + pos = np.array(pos) + ori = np.array(ori) + + pl.figure() + pl.subplot(311) + pl.title('Position') + pl.plot(times, (pos[:,0])) + if (splineB is not None): + pl.plot(times, (posB[:,0])) + + pl.subplot(312) + pl.plot(times, (pos[:,1])) + if (splineB is not None): + pl.plot(times, (posB[:,1])) + + pl.subplot(313) + pl.plot(times, (pos[:,2])) + if (splineB is not None): + pl.plot(times, (posB[:,2])) + + pl.figure() + pl.subplot(311) + pl.title('Orientation') + pl.plot(times, (ori[:,0])) + if (splineB is not None): + pl.plot(times, (oriB[:,0])) + + pl.subplot(312) + pl.plot(times, (ori[:,1])) + if (splineB is not None): + pl.plot(times, (oriB[:,1])) + + pl.subplot(313) + pl.plot(times, (ori[:,2])) + if (splineB is not None): + pl.plot(times, (oriB[:,2])) + + pl.show() + + + +def plotSplineValues(spline, splineB = None): + """Plots a spline over the full range of times. + + Args: + spline (BsplinePose): a pose describing spline + """ + t1 = spline.t_min() + t2 = spline.t_max() + times = np.linspace(t1, t2, 1000) + + pos = [] + for t in times: + v = spline.eval(t) + pos.append(v) + + if (splineB is not None): + posB = [] + for t in times: + v = splineB.eval(t) + posB.append(v) + posB = np.array(posB) + + pos = np.array(pos) + + pl.figure() + pl.subplot(311) + pl.title('Position') + pl.plot(times, (pos[:,0])) + if (splineB is not None): + pl.plot(times, (posB[:,0])) + + pl.subplot(312) + pl.plot(times, (pos[:,1])) + if (splineB is not None): + pl.plot(times, (posB[:,1])) + + pl.subplot(313) + pl.plot(times, (pos[:,2])) + if (splineB is not None): + pl.plot(times, (posB[:,2])) + + pl.figure() + pl.subplot(311) + pl.title('Orientation') + pl.plot(times, (pos[:,3])) + if (splineB is not None): + pl.plot(times, (posB[:,3])) + + pl.subplot(312) + pl.plot(times, (pos[:,4])) + if (splineB is not None): + pl.plot(times, (posB[:,4])) + + pl.subplot(313) + pl.plot(times, (pos[:,5])) + if (splineB is not None): + pl.plot(times, (posB[:,5])) + + pl.show() + diff --git a/aslam_offline_calibration/kalibr/python/kalibr_rs_camera_calibration/__init__.py b/aslam_offline_calibration/kalibr/python/kalibr_rs_camera_calibration/__init__.py new file mode 100644 index 000000000..0940f8237 --- /dev/null +++ b/aslam_offline_calibration/kalibr/python/kalibr_rs_camera_calibration/__init__.py @@ -0,0 +1 @@ +from RsCalibrator import * diff --git a/aslam_optimizer/aslam_backend/include/aslam/backend/DogLegTrustRegionPolicy.hpp b/aslam_optimizer/aslam_backend/include/aslam/backend/DogLegTrustRegionPolicy.hpp index 054fa5a28..dbae3871f 100644 --- a/aslam_optimizer/aslam_backend/include/aslam/backend/DogLegTrustRegionPolicy.hpp +++ b/aslam_optimizer/aslam_backend/include/aslam/backend/DogLegTrustRegionPolicy.hpp @@ -7,32 +7,32 @@ namespace aslam { namespace backend { - + class DogLegTrustRegionPolicy : public TrustRegionPolicy { public: DogLegTrustRegionPolicy(); virtual ~DogLegTrustRegionPolicy(); - + /// \brief called by the optimizer when an optimization is starting virtual void optimizationStartingImplementation(double J); - + // Returns true if the solution was successful - virtual bool solveSystemImplementation(double J, bool previousIterationFailed, int nThreads, Eigen::VectorXd& outDx); - + virtual bool solveSystemImplementation(double J, bool previousIterationFailed, int nThreads, Eigen::VectorXd& outDx); + /// \brief should the optimizer revert on failure? You should probably return true bool revertOnFailure(); - + /// \brief print the current state to a stream (no newlines). virtual std::ostream & printState(std::ostream & out) const; - virtual bool requiresAugmentedDiagonal() const; - virtual std::string name() const { return "dog_leg"; } + virtual bool requiresAugmentedDiagonal() const; + virtual std::string name() const { return "dog_leg"; } private: - + Eigen::VectorXd _dx; Eigen::VectorXd _dx_sd; Eigen::VectorXd _dx_gn; - + double _dx_sd_norm; double _dx_gn_norm; double _L0; @@ -41,9 +41,9 @@ namespace aslam { double _delta; double _p_delta; std::string _stepType; - + }; - + } // namespace backend } // namespace aslam diff --git a/aslam_optimizer/aslam_backend/include/aslam/backend/TrustRegionPolicy.hpp b/aslam_optimizer/aslam_backend/include/aslam/backend/TrustRegionPolicy.hpp index 8678f948a..d105b9f6f 100644 --- a/aslam_optimizer/aslam_backend/include/aslam/backend/TrustRegionPolicy.hpp +++ b/aslam_optimizer/aslam_backend/include/aslam/backend/TrustRegionPolicy.hpp @@ -50,7 +50,10 @@ namespace aslam { private: /// \brief the linear system solver. double _J; + // cost of previous successful step double _p_J; + // cost of the last successful step + double _last_successful_J; bool _isFirstIteration; }; diff --git a/aslam_optimizer/aslam_backend/src/DogLegTrustRegionPolicy.cpp b/aslam_optimizer/aslam_backend/src/DogLegTrustRegionPolicy.cpp index 21d29e74c..4e39cb323 100644 --- a/aslam_optimizer/aslam_backend/src/DogLegTrustRegionPolicy.cpp +++ b/aslam_optimizer/aslam_backend/src/DogLegTrustRegionPolicy.cpp @@ -2,11 +2,11 @@ namespace aslam { namespace backend { - + DogLegTrustRegionPolicy::DogLegTrustRegionPolicy() {} DogLegTrustRegionPolicy::~DogLegTrustRegionPolicy() {} - - + + /// \brief called by the optimizer when an optimization is starting void DogLegTrustRegionPolicy::optimizationStartingImplementation(double /* J */) { @@ -18,21 +18,20 @@ namespace aslam { std::string _stepType; _delta = 0; _p_delta = 0; - + } - + // Returns true if the solution was successful bool DogLegTrustRegionPolicy::solveSystemImplementation(double J, bool previousIterationFailed, int nThreads, Eigen::VectorXd& outDx) { SM_ASSERT_TRUE(Exception, _solver.get() != NULL, "The solver is null"); bool solutionSuccess = true; - + /////////////// ///Update Delta // same check as in GN lambda update: double rho = get_dJ() / _L0; - - + if( ! isFirstIteration() ) { // update trust region @@ -57,29 +56,28 @@ namespace aslam { else _delta /= 2.0; } - } + } bool gnComputed = true; // successful step: // rebuild system and recalculate sd-solution if(!previousIterationFailed) { // update GN matrices: - //std::cout << "Building system\n"; + // std::cout << "Building system\n"; _solver->buildSystem(nThreads, true); - + // calculate steepest descent step: - + _sd_scale = _solver->rhs().squaredNorm() / _solver->rhsJtJrhs(); - // std::cout << " alpha:" << _sd_scale << std::endl; _dx_sd = _sd_scale * _solver->rhs(); - - + + // we need the norm for comparison: _dx_sd_norm = _dx_sd.norm(); - + // invalidate GN solution gnComputed = false; } - + // Trust Region smaller than SD: if (_dx_sd_norm >= _delta && _delta != 0) { @@ -94,15 +92,15 @@ namespace aslam { if(!gnComputed) { solutionSuccess = _solver->solveSystem(_dx_gn); - + if(!solutionSuccess) return solutionSuccess; - + gnComputed = true; // now we have it! // and calculate the norm: _dx_gn_norm = _dx_gn.norm(); } - + // set delta in the first step to take a full GN step: if(_delta == 0) { _delta = (_dx_sd + 0.5 * ( _dx_gn - _dx_sd )).norm(); @@ -119,7 +117,7 @@ namespace aslam { // get beta: Eigen::VectorXd dgnsd = _dx_gn - _dx_sd; double gdnsd_norm_sqr = dgnsd.squaredNorm(); - + double c = _dx_sd.transpose() * ( dgnsd ); if(c <= 0) { @@ -131,18 +129,18 @@ namespace aslam { _beta = (_delta*_delta - _dx_sd_norm*_dx_sd_norm); _beta /= c + sqrt( c*c + gdnsd_norm_sqr*(_delta*_delta - _dx_sd_norm*_dx_sd_norm) ); } - + _dx = _dx_sd + _beta * ( dgnsd ); _L0 = 1/2 * _sd_scale * (1-_beta)*(1-_beta)* _solver->rhs().squaredNorm() + _beta*(2-_beta)*J; _stepType = "DL"; } } - + outDx = _dx; return solutionSuccess; - + } - + /// \brief print the current state to a stream (no newlines). std::ostream & DogLegTrustRegionPolicy::printState(std::ostream & out) const { @@ -152,15 +150,15 @@ namespace aslam { return out; } - + bool DogLegTrustRegionPolicy::revertOnFailure() { return true; } - bool DogLegTrustRegionPolicy::requiresAugmentedDiagonal() const { - return false; - } - + bool DogLegTrustRegionPolicy::requiresAugmentedDiagonal() const { + return false; + } + } // namespace backend } // namespace aslam diff --git a/aslam_optimizer/aslam_backend/src/Optimizer2.cpp b/aslam_optimizer/aslam_backend/src/Optimizer2.cpp index 65c854e3e..0ec5964e1 100644 --- a/aslam_optimizer/aslam_backend/src/Optimizer2.cpp +++ b/aslam_optimizer/aslam_backend/src/Optimizer2.cpp @@ -56,7 +56,7 @@ namespace aslam { { _problem = problem; } - + void Optimizer2::initializeTrustRegionPolicy() { if( !_options.trustRegionPolicy ) { @@ -65,16 +65,16 @@ namespace aslam { } else { _trustRegionPolicy = _options.trustRegionPolicy; } - - + + // \todo remove this check when the sparse qr solver supports an augmented diagonal if(_solver->name() == "sparse_qr" && _trustRegionPolicy->name() == "levenberg_marquardt") { _options.verbose && std::cout << "The sparse_qr solver is not compatible with levenberg_marquardt. Changing to the dog_leg trust region policy\n"; _trustRegionPolicy.reset( new DogLegTrustRegionPolicy() ); } - + _options.verbose && std::cout << "Using the " << _trustRegionPolicy->name() << " trust region policy\n"; - + } @@ -147,7 +147,7 @@ namespace aslam { // \todo initialize the trust region stuff. - + } @@ -191,7 +191,7 @@ namespace aslam { initialize(); SolutionReturnValue srv; _p_J = 0.0; - + //std::cout << "Evaluate error for the first time\n"; // This sets _J timeErr.start(); @@ -217,12 +217,11 @@ namespace aslam { ((deltaX > _options.convergenceDeltaX && fabs(deltaJ) > _options.convergenceDeltaJ) || linearSolverFailure)) { - + timeSolve.start(); bool solutionSuccess = _trustRegionPolicy->solveSystem(_J, previousIterationFailed, _options.nThreads, _dx); timeSolve.stop(); - if (!solutionSuccess) { _options.verbose && std::cout << "[WARNING] System solution failed\n"; previousIterationFailed = true; @@ -263,8 +262,9 @@ namespace aslam { _options.verbose && std::cout << "[" << srv.iterations << "]: J: " << _J << ", dJ: " << deltaJ << ", deltaX: " << deltaX << ", "; _options.verbose && _trustRegionPolicy->printState(std::cout); _options.verbose && std::cout << std::endl; - } - } // if the linear solver failed / else + } // if the linear solver failed / else + } + srv.JFinal = _p_J; srv.dXFinal = deltaX; srv.dJFinal = deltaJ; @@ -387,12 +387,12 @@ namespace aslam { void Optimizer2::computeHessian(SparseBlockMatrix& outH, double lambda) { - + boost::shared_ptr solver_sp; solver_sp.reset(new BlockCholeskyLinearSystemSolver()); // True here for creating the diagonal conditioning. solver_sp->initMatrixStructure(_designVariables, _errorTerms, true); - + _options.verbose && std::cout << "Setting the diagonal conditioner to: " << lambda << ".\n"; evaluateError(false); solver_sp->setConstantConditioner(lambda); diff --git a/aslam_optimizer/aslam_backend/src/TrustRegionPolicy.cpp b/aslam_optimizer/aslam_backend/src/TrustRegionPolicy.cpp index 74e7cb4b7..089ea2632 100644 --- a/aslam_optimizer/aslam_backend/src/TrustRegionPolicy.cpp +++ b/aslam_optimizer/aslam_backend/src/TrustRegionPolicy.cpp @@ -2,10 +2,10 @@ namespace aslam { namespace backend { - + TrustRegionPolicy::TrustRegionPolicy(){} TrustRegionPolicy::~TrustRegionPolicy(){} - + /// \brief get the linear system solver boost::shared_ptr TrustRegionPolicy::getSolver() @@ -18,30 +18,33 @@ namespace aslam { { _solver = solver; } - + bool TrustRegionPolicy::revertOnFailure() { return true; } - + /// \brief called by the optimizer when an optimization is starting void TrustRegionPolicy::optimizationStarting(double J) { _J = J; _p_J = J; - _isFirstIteration=true; + _last_successful_J = J; + _isFirstIteration = true; optimizationStartingImplementation(J); } - + // Returns true if the solution was successful - bool TrustRegionPolicy::solveSystem(double J, bool previousIterationFailed, int nThreads, Eigen::VectorXd& outDx) + bool TrustRegionPolicy::solveSystem(double J, bool previousIterationFailed, int nThreads, Eigen::VectorXd& outDx) { - if(!previousIterationFailed) - { - _p_J = _J; + if(previousIterationFailed) { + _J = J; + } else { + _p_J = _last_successful_J; + _last_successful_J = J; + _J = J; } - _J = J; bool success = solveSystemImplementation(J, previousIterationFailed, nThreads, outDx); _isFirstIteration = false; @@ -52,7 +55,7 @@ namespace aslam { { return _p_J - _J; } - + } // namespace backend diff --git a/aslam_optimizer/aslam_backend_expressions/CMakeLists.txt b/aslam_optimizer/aslam_backend_expressions/CMakeLists.txt index db40c8a8d..ed16dad49 100644 --- a/aslam_optimizer/aslam_backend_expressions/CMakeLists.txt +++ b/aslam_optimizer/aslam_backend_expressions/CMakeLists.txt @@ -14,22 +14,27 @@ include_directories(${EIGEN_INCLUDE_DIRS}) find_package(Boost REQUIRED COMPONENTS system) cs_add_library(${PROJECT_NAME} - src/RotationExpression.cpp + src/RotationExpression.cpp src/RotationExpressionNode.cpp src/RotationQuaternion.cpp src/MappedRotationQuaternion.cpp src/Vector2RotationQuaternionExpressionAdapter.cpp - src/TransformationExpression.cpp + src/MatrixExpression.cpp + src/MatrixExpressionNode.cpp + src/MatrixTransformation.cpp + + src/TransformationExpression.cpp + src/TransformationExpressionNode.cpp src/TransformationBasic.cpp - src/EuclideanExpression.cpp + src/EuclideanExpression.cpp src/EuclideanExpressionNode.cpp src/EuclideanPoint.cpp src/MappedEuclideanPoint.cpp - src/HomogeneousExpression.cpp + src/HomogeneousExpression.cpp src/HomogeneousExpressionNode.cpp src/HomogeneousPoint.cpp src/MappedHomogeneousPoint.cpp @@ -61,7 +66,7 @@ catkin_add_gtest(${PROJECT_NAME}_test test/HomogeneousExpression.cpp test/MatrixAndEuclideanExpression.cpp # \todo reenable after I can talk to Hannes. - test/FixedPointNumberTest.cpp + # test/FixedPointNumberTest.cpp test/GenericScalarExpressionTest.cpp test/GenericMatrixExpression.cpp test/QuaternionExpression.cpp diff --git a/aslam_optimizer/aslam_backend_expressions/include/aslam/backend/GenericScalar.hpp b/aslam_optimizer/aslam_backend_expressions/include/aslam/backend/GenericScalar.hpp index a20b88857..35769c484 100644 --- a/aslam_optimizer/aslam_backend_expressions/include/aslam/backend/GenericScalar.hpp +++ b/aslam_optimizer/aslam_backend_expressions/include/aslam/backend/GenericScalar.hpp @@ -35,7 +35,7 @@ class GenericScalar : public GenericScalarExpressionNode, public Design virtual void updateImplementation(const double * dp, int size); /// \brief the size of an update step - virtual inline constexpr int minimalDimensionsImplementation() const { return MinimalDimension; } + virtual inline int minimalDimensionsImplementation() const { return MinimalDimension; } private: virtual Scalar toScalarImplementation() const; virtual void evaluateJacobiansImplementation(JacobianContainer & outJacobians, const Eigen::MatrixXd * applyChainRule) const; diff --git a/aslam_optimizer/aslam_backend_expressions/include/aslam/backend/HomogeneousExpression.hpp b/aslam_optimizer/aslam_backend_expressions/include/aslam/backend/HomogeneousExpression.hpp index 6a2ddb5f2..ee1ce002a 100644 --- a/aslam_optimizer/aslam_backend_expressions/include/aslam/backend/HomogeneousExpression.hpp +++ b/aslam_optimizer/aslam_backend_expressions/include/aslam/backend/HomogeneousExpression.hpp @@ -35,7 +35,7 @@ namespace aslam { void evaluateJacobians(JacobianContainer & outJacobians, const Eigen::MatrixXd & applyChainRule) const; void getDesignVariables(DesignVariable::set_t & designVariables) const; - boost::shared_ptr root() { return _root; } + boost::shared_ptr root() const { return _root; } private: friend class TransformationExpression; diff --git a/aslam_optimizer/aslam_backend_expressions/include/aslam/backend/test/RotationExpressionTests.hpp b/aslam_optimizer/aslam_backend_expressions/include/aslam/backend/test/RotationExpressionTests.hpp index 1d5806544..edf3f5cf6 100644 --- a/aslam_optimizer/aslam_backend_expressions/include/aslam/backend/test/RotationExpressionTests.hpp +++ b/aslam_optimizer/aslam_backend_expressions/include/aslam/backend/test/RotationExpressionTests.hpp @@ -89,7 +89,13 @@ class ExpressionJacobianTestTraits { Eigen::MatrixXd Jest = numdiff.estimateJacobian(dp); auto JcM = Jc.asSparseMatrix(); - sm::eigen::assertNear(Jc.asSparseMatrix(), Jest, expressionTester.getTolerance(), SM_SOURCE_FILE_POS, static_cast(std::stringstream("Testing the RotationExpression's Jacobian (column=") << i <<")").str()); + sm::eigen::assertNear( + Jc.asSparseMatrix(), + Jest, + expressionTester.getTolerance(), + SM_SOURCE_FILE_POS, + (std::stringstream("Testing the RotationExpression's Jacobian (column=") << i << ")").str() + ); if (expressionTester.getPrintResult()) { std::cout << "Jest=\n" << Jest << std::endl; std::cout << "Jc=\n" << JcM << std::endl; diff --git a/aslam_optimizer/aslam_backend_expressions/include/aslam/backend/test/RotationQuaternionAsVectorExpressionNode.hpp b/aslam_optimizer/aslam_backend_expressions/include/aslam/backend/test/RotationQuaternionAsVectorExpressionNode.hpp index acf22cd54..b7848df1a 100644 --- a/aslam_optimizer/aslam_backend_expressions/include/aslam/backend/test/RotationQuaternionAsVectorExpressionNode.hpp +++ b/aslam_optimizer/aslam_backend_expressions/include/aslam/backend/test/RotationQuaternionAsVectorExpressionNode.hpp @@ -24,7 +24,11 @@ struct RotationQuaternionAsVectorExpressionNode : public VectorExpressionNode<4> virtual void evaluateJacobiansImplementation(JacobianContainer & outJacobians) const { evaluateJacobiansImplementation(outJacobians, Eigen::Matrix3d::Identity()); }; virtual void evaluateJacobiansImplementation(JacobianContainer & outJacobians, const Eigen::MatrixXd & applyChainRule) const { if(AssumeLieAlgebraVectorInputInJacobian){ - quat.evaluateJacobians(outJacobians, applyChainRule * sm::kinematics::quatOPlus(quat.getQuaternion() * 0.5).topLeftCorner<4,3>()); + Eigen::Matrix4d tmp = sm::kinematics::quatOPlus(quat.getQuaternion() * 0.5); + quat.evaluateJacobians( + outJacobians, + applyChainRule * tmp.topLeftCorner<4,3>() + ); }else{ quat.evaluateJacobians(outJacobians, applyChainRule); } diff --git a/aslam_optimizer/aslam_backend_python/include/aslam/python/ExportDesignVariableAdapter.hpp b/aslam_optimizer/aslam_backend_python/include/aslam/python/ExportDesignVariableAdapter.hpp index 194d28447..56d5e6632 100644 --- a/aslam_optimizer/aslam_backend_python/include/aslam/python/ExportDesignVariableAdapter.hpp +++ b/aslam_optimizer/aslam_backend_python/include/aslam/python/ExportDesignVariableAdapter.hpp @@ -26,6 +26,7 @@ namespace aslam { class_< DesignVariableAdapter, boost::shared_ptr >, bases >( name.c_str(), init >() ) .def("value", &DesignVariableAdapter::valuePtr) ; + register_ptr_to_python > >(); //boost::python::implicitly_convertible >, boost::shared_ptr >(); }