├── test ├── PoseCeresTest.cpp ├── test_main.cpp ├── QuaternionOmegaCeresTest.cpp ├── CMakeLists.txt └── UtilityTest.cpp ├── .gitignore ├── README.md ├── data └── MH_01_easy │ └── state_groundtruth_estimate0 │ └── .DS_Store ├── src ├── VectorSplineSampleAutoError.cpp ├── internal │ ├── utility.cpp │ └── pose_local_parameterization.cpp ├── maplab │ └── quaternion_param_jpl.cpp ├── extern │ ├── spline_projection_error3.cpp │ ├── spline_projection_error4.cpp │ ├── project_error.cpp │ ├── spline_projection_error.cpp │ ├── spline_projection_error1.cpp │ ├── spline_projection_error2.cpp │ └── pinhole_project_error.cpp ├── OmegaExtrinsicTemperalError.cpp ├── QuaternionOmegaSampleError.cpp ├── Duration.cpp ├── PoseSplineSampleTemporalError.cpp ├── VectorSplineSampleVelocityError.cpp └── QuaternionSplineUtility.cpp ├── include ├── PoseSpline │ ├── PoseSpline.hpp │ ├── maplab │ │ ├── pose_param_jpl.h │ │ ├── quaternion_param_jpl.h │ │ ├── quaternion-math.h │ │ └── geometry.h │ ├── TypeTraits.hpp │ ├── QuaternionSpline.hpp │ ├── OmegaExtrinsicTemperalError.hpp │ ├── AngularVelocitySampleError.hpp │ ├── PoseSplineSampleError.hpp │ ├── VectorSplineSampleVelocityError.hpp │ ├── QuaternionSplineSampleError.hpp │ ├── VectorSplineSampleAutoError.hpp │ ├── QuaternionSplineSampleAutoError.hpp │ ├── QuaternionLocalParameter.hpp │ ├── PoseSplineUtility.hpp │ ├── PoseLocalParameter.hpp │ ├── ErrorInterface.hpp │ ├── PoseSplineSampleTemporalError.hpp │ ├── QuaternionOmegaSampleError.hpp │ ├── VectorSplineSampleError.hpp │ ├── LinearAccelerateSampleError.hpp │ ├── Time_imp.hpp │ ├── Duration_imp.hpp │ ├── QuaternionError.hpp │ ├── Duration.hpp │ ├── VectorSpaceSpline.hpp │ ├── NumbDifferentiator.hpp │ └── BSplineBase.hpp ├── internal │ ├── pose_local_parameterization.h │ └── utility.h └── extern │ ├── RotateVectorError.hpp │ ├── TransformVectorError.hpp │ ├── project_error.h │ ├── pinhole_project_error.h │ ├── spline_projection_error.h │ ├── PoseParameterBlock.hpp │ ├── spline_projection_error_simple.h │ └── spline_projection_error1.h ├── package.xml ├── cmake_modules ├── FindGlog.cmake └── FindEigen3.cmake └── CMakeLists.txt /test/PoseCeresTest.cpp: -------------------------------------------------------------------------------- 1 | -------------------------------------------------------------------------------- /.gitignore: -------------------------------------------------------------------------------- 1 | .idea/* 2 | cmake-build-debug/* 3 | cmake-build-release/* 4 | build/* 5 | *~ 6 | 7 | .vscode/ 8 | -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | # PoseSpline 2 | A project to reproduce the method from " Continuous-Time Estimation of attitude 3 | using B-splines on Lie groups " 4 | -------------------------------------------------------------------------------- /data/MH_01_easy/state_groundtruth_estimate0/.DS_Store: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/pangfumin/PoseSpline/HEAD/data/MH_01_easy/state_groundtruth_estimate0/.DS_Store -------------------------------------------------------------------------------- /test/test_main.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | 3 | /// Run all the tests that were declared with TEST() 4 | int main(int argc, char **argv){ 5 | testing::InitGoogleTest(&argc, argv); 6 | return RUN_ALL_TESTS(); 7 | } 8 | -------------------------------------------------------------------------------- /test/QuaternionOmegaCeresTest.cpp: -------------------------------------------------------------------------------- 1 | #include "PoseSpline/QuaternionOmegaSampleError.hpp" 2 | #include "PoseSpline/NumbDifferentiator.hpp" 3 | 4 | #include "PoseSpline/QuaternionSplineUtility.hpp" 5 | #include "PoseSpline/QuaternionLocalParameter.hpp" 6 | 7 | int main(int argc, char** argv){ 8 | google::InitGoogleLogging(argv[0]); 9 | 10 | 11 | 12 | 13 | 14 | return 0; 15 | } -------------------------------------------------------------------------------- /src/VectorSplineSampleAutoError.cpp: -------------------------------------------------------------------------------- 1 | #include "PoseSpline/VectorSplineSampleAutoError.hpp" 2 | 3 | 4 | 5 | VectorSplineSampleAutoError::VectorSplineSampleAutoError(const double& t_meas, 6 | const Eigen::Vector3d& V_meas): 7 | t_meas_(t_meas),V_Meas_(V_meas){ 8 | 9 | }; 10 | 11 | VectorSplineSampleAutoError::~VectorSplineSampleAutoError(){ 12 | 13 | } 14 | -------------------------------------------------------------------------------- /include/PoseSpline/PoseSpline.hpp: -------------------------------------------------------------------------------- 1 | #ifndef POSE_SPLINE_H_ 2 | #define POSE_SPLINE_H_ 3 | 4 | 5 | #include "PoseSpline/BSplineBase.hpp" 6 | #include "PoseSpline/Pose.hpp" 7 | 8 | 9 | class PoseSpline : public BSplineBase, 4> { 10 | 11 | public: 12 | PoseSpline(); 13 | 14 | PoseSpline( double interval); 15 | 16 | virtual ~PoseSpline() {} 17 | 18 | void initialPoseSpline(std::vector>> Meas) ; 19 | 20 | Pose evalPoseSpline(real_t t); 21 | Eigen::Vector3d evalLinearVelocity(real_t t ); 22 | 23 | Eigen::Vector3d evalLinearAccelerator(real_t t, const Eigen::Vector3d& gravity); 24 | Eigen::Vector3d evalOmega(real_t t); 25 | }; 26 | #endif -------------------------------------------------------------------------------- /include/PoseSpline/maplab/pose_param_jpl.h: -------------------------------------------------------------------------------- 1 | #ifndef CERES_ERROR_TERMS_PARAMETERIZATION_POSE_PARAM_JPL_H_ 2 | #define CERES_ERROR_TERMS_PARAMETERIZATION_POSE_PARAM_JPL_H_ 3 | 4 | #include 5 | #include 6 | 7 | #include "PoseSpline/maplab/quaternion_param_jpl.h" 8 | 9 | namespace ceres_error_terms { 10 | 11 | class JplPoseParameterization : public ceres::ProductParameterization { 12 | public: 13 | JplPoseParameterization() 14 | : ceres::ProductParameterization( 15 | new JplQuaternionParameterization, 16 | new ceres::IdentityParameterization(3) 17 | ) {} 18 | virtual ~JplPoseParameterization() {} 19 | }; 20 | 21 | 22 | } // namespace ceres_error_terms 23 | 24 | #endif // CERES_ERROR_TERMS_PARAMETERIZATION_POSE_PARAM_JPL_H_ 25 | -------------------------------------------------------------------------------- /src/internal/utility.cpp: -------------------------------------------------------------------------------- 1 | /******************************************************* 2 | * Copyright (C) 2019, Aerial Robotics Group, Hong Kong University of Science and Technology 3 | * 4 | * This file is part of VINS. 5 | * 6 | * Licensed under the GNU General Public License v3.0; 7 | * you may not use this file except in compliance with the License. 8 | *******************************************************/ 9 | 10 | #include "internal/utility.h" 11 | 12 | Eigen::Matrix3d Utility::g2R(const Eigen::Vector3d &g) 13 | { 14 | Eigen::Matrix3d R0; 15 | Eigen::Vector3d ng1 = g.normalized(); 16 | Eigen::Vector3d ng2{0, 0, 1.0}; 17 | R0 = Eigen::Quaterniond::FromTwoVectors(ng1, ng2).toRotationMatrix(); 18 | double yaw = Utility::R2ypr(R0).x(); 19 | R0 = Utility::ypr2R(Eigen::Vector3d{-yaw, 0, 0}) * R0; 20 | // R0 = Utility::ypr2R(Eigen::Vector3d{-90, 0, 0}) * R0; 21 | return R0; 22 | } 23 | -------------------------------------------------------------------------------- /package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | PoseSpline 4 | 0.0.0 5 | PoseSpline 6 | 7 | Helen Oleynikova 8 | Marius Fehr 9 | 10 | Helen Oleynikova 11 | Marius Fehr 12 | 13 | BSD 14 | 15 | catkin 16 | catkin_simple 17 | 18 | eigen_catkin 19 | eigen_checks 20 | gflags_catkin 21 | glog_catkin 22 | opencv3_catkin 23 | 24 | ceres_catkin 25 | 26 | -------------------------------------------------------------------------------- /include/PoseSpline/TypeTraits.hpp: -------------------------------------------------------------------------------- 1 | #ifndef _TYPE_TRAITS_H_ 2 | #define _TYPE_TRAITS_H_ 3 | 4 | #include 5 | #include "Pose.hpp" 6 | template 7 | class TypeTraits; 8 | 9 | template <> 10 | class TypeTraits { 11 | public: 12 | typedef Quaternion TypeT; 13 | enum {Dim = 4, miniDim = 3}; 14 | static TypeT zero() { 15 | return unitQuat(); 16 | } 17 | }; 18 | 19 | 20 | template <> 21 | class TypeTraits> { 22 | public: 23 | typedef Pose TypeT; 24 | enum {Dim = 7, miniDim = 6}; 25 | static TypeT zero() { 26 | return Pose(); 27 | } 28 | }; 29 | 30 | 31 | template 32 | class TypeTraits> { 33 | public: 34 | typedef Eigen::Matrix TypeT; 35 | enum {Dim = D, miniDim = D}; 36 | static TypeT zero() { 37 | return Eigen::Matrix::Zero(); 38 | } 39 | }; 40 | 41 | 42 | #endif -------------------------------------------------------------------------------- /include/internal/pose_local_parameterization.h: -------------------------------------------------------------------------------- 1 | /******************************************************* 2 | * Copyright (C) 2019, Aerial Robotics Group, Hong Kong University of Science and Technology 3 | * 4 | * This file is part of VINS. 5 | * 6 | * Licensed under the GNU General Public License v3.0; 7 | * you may not use this file except in compliance with the License. 8 | *******************************************************/ 9 | 10 | #pragma once 11 | 12 | #include 13 | #include 14 | #include "utility.h" 15 | namespace hamilton { 16 | class PoseLocalParameterization : public ceres::LocalParameterization { 17 | public: 18 | virtual bool Plus(const double *x, const double *delta, double *x_plus_delta) const; 19 | 20 | virtual bool ComputeJacobian(const double *x, double *jacobian) const; 21 | 22 | virtual int GlobalSize() const { return 7; }; 23 | 24 | virtual int LocalSize() const { return 6; }; 25 | }; 26 | } 27 | -------------------------------------------------------------------------------- /include/PoseSpline/QuaternionSpline.hpp: -------------------------------------------------------------------------------- 1 | #ifndef QUATERNIONSPLINE_H 2 | #define QUATERNIONSPLINE_H 3 | 4 | #include "Quaternion.hpp" 5 | #include "PoseSpline/BSplineBase.hpp" 6 | 7 | 8 | class QuaternionSpline : public BSplineBase { 9 | 10 | public: 11 | typedef BSplineBase base_t; 12 | QuaternionSpline(); 13 | QuaternionSpline(double interval); 14 | 15 | virtual ~QuaternionSpline(); 16 | 17 | void initialQuaternionSpline(std::vector> Meas); 18 | 19 | 20 | 21 | void printKnots(); 22 | Quaternion evalQuatSpline(real_t t); 23 | Quaternion evalDotQuatSpline(real_t t); 24 | Quaternion evalDotDotQuatSpline(real_t t); 25 | 26 | void evalQuatSplineDerivate(real_t t, 27 | double* Quat, 28 | double* dot_Qaut = NULL, 29 | double* dot_dot_Quat = NULL); 30 | 31 | 32 | Eigen::Vector3d evalOmega(real_t t); 33 | Eigen::Vector3d evalAlpha(real_t t); 34 | 35 | Eigen::Vector3d evalNumRotOmega(real_t t); 36 | 37 | 38 | private: 39 | 40 | 41 | }; 42 | 43 | #endif -------------------------------------------------------------------------------- /include/PoseSpline/OmegaExtrinsicTemperalError.hpp: -------------------------------------------------------------------------------- 1 | #ifndef OMEGAEXTRINSICTEMPERALERROR_H 2 | #define OMEGAEXTRINSICTEMPERALERROR_H 3 | 4 | #include "PoseSpline/QuaternionSpline.hpp" 5 | #include 6 | #include 7 | #include "PoseSpline/QuaternionLocalParameter.hpp" 8 | #include "PoseSpline/ErrorInterface.hpp" 9 | 10 | class OmegaExtrinsicTemperalError: public ceres::SizedCostFunction<3,4,1>{ 11 | 12 | OmegaExtrinsicTemperalError(); 13 | OmegaExtrinsicTemperalError(const Eigen::Vector3d& omega_meas, 14 | const Quaternion& Q_cw, 15 | const Quaternion& dotQ_cw, 16 | const Quaternion& dotdotQ_cw); 17 | 18 | virtual ~OmegaExtrinsicTemperalError(); 19 | 20 | virtual bool Evaluate(double const* const* parameters, 21 | double* residuals, 22 | double** jacobians) const; 23 | bool EvaluateWithMinimalJacobians(double const* const * parameters, 24 | double* residuals, 25 | double** jacobians, 26 | double** jacobiansMinimal) const; 27 | 28 | private: 29 | Eigen::Vector3d omega_meas_; 30 | Quaternion Q_cw_; 31 | Quaternion dotQ_cw_; 32 | Quaternion dotdotQ_cw_; 33 | }; 34 | 35 | #endif -------------------------------------------------------------------------------- /include/PoseSpline/AngularVelocitySampleError.hpp: -------------------------------------------------------------------------------- 1 | #ifndef ANGULAR_VELOCITY_SAMPLE_ERROR 2 | #define ANGULAR_VELOCITY_SAMPLE_ERROR 3 | 4 | #include 5 | #include "PoseSpline/QuaternionSplineUtility.hpp" 6 | #include "PoseSpline/QuaternionOmegaSampleError.hpp" 7 | 8 | class AngularVelocitySampleAutoError : public ceres::SizedCostFunction<3, 9 | 7, 7, 7, 7,3,3,3,3> { 10 | public: 11 | explicit AngularVelocitySampleAutoError(QuaternionOmegaSampleFunctor* functor) 12 | : functor_(functor) { 13 | 14 | } 15 | 16 | virtual ~AngularVelocitySampleAutoError() {} 17 | 18 | // Implementation details follow; clients of the autodiff cost function should 19 | // not have to examine below here. 20 | // 21 | // To handle varardic cost functions, some template magic is needed. It's 22 | // mostly hidden inside autodiff.h. 23 | virtual bool Evaluate(double const* const* parameters, 24 | double* residuals, 25 | double** jacobians) const; 26 | 27 | bool EvaluateWithMinimalJacobians(double const* const * parameters, 28 | double* residuals, 29 | double** jacobians, 30 | double** jacobiansMinimal) const; 31 | 32 | private: 33 | QuaternionOmegaSampleFunctor* functor_; 34 | }; 35 | 36 | #endif 37 | -------------------------------------------------------------------------------- /include/PoseSpline/PoseSplineSampleError.hpp: -------------------------------------------------------------------------------- 1 | #ifndef POSESPLINESAMPLEERROR_H 2 | #define POSESPLINESAMPLEERROR_H 3 | 4 | #include "PoseSpline/QuaternionSpline.hpp" 5 | #include 6 | #include 7 | #include "PoseSpline/QuaternionLocalParameter.hpp" 8 | #include "PoseSpline/ErrorInterface.hpp" 9 | #include "PoseSpline/Pose.hpp" 10 | 11 | 12 | class PoseSplineSampleError 13 | : public ceres::SizedCostFunction<6, 7, 7, 7, 7> { 14 | public: 15 | 16 | typedef Eigen::Matrix covariance_t; 17 | typedef covariance_t information_t; 18 | 19 | PoseSplineSampleError(double t_meas, Pose T_meas); 20 | virtual ~PoseSplineSampleError(); 21 | 22 | virtual bool Evaluate(double const* const* parameters, 23 | double* residuals, 24 | double** jacobians) const; 25 | bool EvaluateWithMinimalJacobians(double const* const * parameters, 26 | double* residuals, 27 | double** jacobians, 28 | double** jacobiansMinimal) const; 29 | private: 30 | 31 | double t_meas_; 32 | Pose T_Meas_; 33 | mutable information_t information_; ///< The information matrix for this error term. 34 | mutable information_t squareRootInformation_; ///< The square root information matrix for this error term. 35 | }; 36 | 37 | #endif -------------------------------------------------------------------------------- /src/internal/pose_local_parameterization.cpp: -------------------------------------------------------------------------------- 1 | /******************************************************* 2 | * Copyright (C) 2019, Aerial Robotics Group, Hong Kong University of Science and Technology 3 | * 4 | * This file is part of VINS. 5 | * 6 | * Licensed under the GNU General Public License v3.0; 7 | * you may not use this file except in compliance with the License. 8 | *******************************************************/ 9 | 10 | #include "internal/pose_local_parameterization.h" 11 | namespace hamilton { 12 | bool PoseLocalParameterization::Plus(const double *x, const double *delta, double *x_plus_delta) const { 13 | Eigen::Map _p(x); 14 | Eigen::Map _q(x + 3); 15 | 16 | Eigen::Map dp(delta); 17 | 18 | Eigen::Quaterniond dq = Utility::deltaQ(Eigen::Map(delta + 3)); 19 | 20 | Eigen::Map p(x_plus_delta); 21 | Eigen::Map q(x_plus_delta + 3); 22 | 23 | p = _p + dp; 24 | q = (_q * dq).normalized(); 25 | 26 | return true; 27 | } 28 | 29 | bool PoseLocalParameterization::ComputeJacobian(const double *x, double *jacobian) const { 30 | Eigen::Map> j(jacobian); 31 | j.topRows<6>().setIdentity(); 32 | j.bottomRows<1>().setZero(); 33 | 34 | return true; 35 | } 36 | } -------------------------------------------------------------------------------- /include/extern/RotateVectorError.hpp: -------------------------------------------------------------------------------- 1 | #ifndef RORATEVECTORERROR_H 2 | #define RORATEVECTORERROR_H 3 | 4 | 5 | #include "PoseSpline/QuaternionSpline.hpp" 6 | #include 7 | #include 8 | #include "PoseSpline/QuaternionLocalParameter.hpp" 9 | #include "PoseSpline/ErrorInterface.hpp" 10 | 11 | 12 | class RoatateVectorError 13 | : public ceres::SizedCostFunction<3, 4, 4, 4, 4> { 14 | public: 15 | 16 | typedef Eigen::Matrix covariance_t; 17 | typedef covariance_t information_t; 18 | 19 | RoatateVectorError(double t_meas, Eigen::Vector3d originalVector, 20 | Eigen::Vector3d rotatedVector); 21 | virtual ~RoatateVectorError(); 22 | 23 | virtual bool Evaluate(double const* const* parameters, 24 | double* residuals, 25 | double** jacobians) const; 26 | bool EvaluateWithMinimalJacobians(double const* const * parameters, 27 | double* residuals, 28 | double** jacobians, 29 | double** jacobiansMinimal) const; 30 | 31 | 32 | private: 33 | 34 | double t_meas_; 35 | Eigen::Vector3d rotatedVector_Meas_; 36 | Eigen::Vector3d originalVector_; 37 | mutable information_t information_; ///< The information matrix for this error term. 38 | mutable information_t squareRootInformation_; ///< The square root information matrix for this error term. 39 | }; 40 | 41 | #endif -------------------------------------------------------------------------------- /include/PoseSpline/VectorSplineSampleVelocityError.hpp: -------------------------------------------------------------------------------- 1 | #ifndef VECTORSPLINESAMPLEVELOCITYERROR_H 2 | #define VECTORSPLINESAMPLEVELOCITYERROR_H 3 | 4 | #include "PoseSpline/QuaternionSpline.hpp" 5 | #include 6 | #include 7 | #include "PoseSpline/QuaternionLocalParameter.hpp" 8 | #include "PoseSpline/ErrorInterface.hpp" 9 | 10 | 11 | class VectorSplineSampleVelocityError: public ceres::SizedCostFunction<3,3,3,3,3>{ 12 | public: 13 | typedef Eigen::Matrix covariance_t; 14 | typedef covariance_t information_t; 15 | 16 | VectorSplineSampleVelocityError(const double& t_meas, const double& time_interval, const Eigen::Vector3d& V_meas); 17 | virtual ~VectorSplineSampleVelocityError(); 18 | 19 | virtual bool Evaluate(double const* const* parameters, 20 | double* residuals, 21 | double** jacobians) const; 22 | bool EvaluateWithMinimalJacobians(double const* const * parameters, 23 | double* residuals, 24 | double** jacobians, 25 | double** jacobiansMinimal) const; 26 | 27 | 28 | private: 29 | 30 | double t_meas_; 31 | double time_interval_; 32 | Eigen::Vector3d V_Meas_; 33 | mutable information_t information_; ///< The information matrix for this error term. 34 | mutable information_t squareRootInformation_; ///< The square root information matrix for this error term. 35 | }; 36 | 37 | #endif -------------------------------------------------------------------------------- /include/extern/TransformVectorError.hpp: -------------------------------------------------------------------------------- 1 | #ifndef TRAMSFORMVECTORERROR_H 2 | #define TRAMSFORMVECTORERROR_H 3 | 4 | 5 | #include "PoseSpline/QuaternionSpline.hpp" 6 | #include 7 | #include 8 | #include "PoseSpline/QuaternionLocalParameter.hpp" 9 | #include "PoseSpline/ErrorInterface.hpp" 10 | 11 | 12 | class TransformVectorError 13 | : public ceres::SizedCostFunction<3, 7, 7, 7, 7> { 14 | public: 15 | 16 | typedef Eigen::Matrix covariance_t; 17 | typedef covariance_t information_t; 18 | 19 | TransformVectorError(double t_meas, Eigen::Vector3d originalVector, 20 | Eigen::Vector3d rotatedVector); 21 | virtual ~TransformVectorError(); 22 | 23 | virtual bool Evaluate(double const* const* parameters, 24 | double* residuals, 25 | double** jacobians) const; 26 | bool EvaluateWithMinimalJacobians(double const* const * parameters, 27 | double* residuals, 28 | double** jacobians, 29 | double** jacobiansMinimal) const; 30 | 31 | 32 | private: 33 | 34 | double t_meas_; 35 | Eigen::Vector3d transformedVector_Meas_; 36 | Eigen::Vector3d originalVector_; 37 | mutable information_t information_; ///< The information matrix for this error term. 38 | mutable information_t squareRootInformation_; ///< The square root information matrix for this error term. 39 | }; 40 | 41 | #endif -------------------------------------------------------------------------------- /cmake_modules/FindGlog.cmake: -------------------------------------------------------------------------------- 1 | # - Try to find Glog 2 | # 3 | # The following variables are optionally searched for defaults 4 | # GLOG_ROOT_DIR: Base directory where all GLOG components are found 5 | # 6 | # The following are set after configuration is done: 7 | # GLOG_FOUND 8 | # GLOG_INCLUDE_DIRS 9 | # GLOG_LIBRARIES 10 | # GLOG_LIBRARYRARY_DIRS 11 | 12 | include(FindPackageHandleStandardArgs) 13 | 14 | set(GLOG_ROOT_DIR "" CACHE PATH "Folder contains Google glog") 15 | 16 | if(WIN32) 17 | find_path(GLOG_INCLUDE_DIR glog/logging.h 18 | PATHS ${GLOG_ROOT_DIR}/src/windows) 19 | else() 20 | find_path(GLOG_INCLUDE_DIR glog/logging.h 21 | PATHS ${GLOG_ROOT_DIR}) 22 | endif() 23 | 24 | if(MSVC) 25 | find_library(GLOG_LIBRARY_RELEASE libglog_static 26 | PATHS ${GLOG_ROOT_DIR} 27 | PATH_SUFFIXES Release) 28 | 29 | find_library(GLOG_LIBRARY_DEBUG libglog_static 30 | PATHS ${GLOG_ROOT_DIR} 31 | PATH_SUFFIXES Debug) 32 | 33 | set(GLOG_LIBRARY optimized ${GLOG_LIBRARY_RELEASE} debug ${GLOG_LIBRARY_DEBUG}) 34 | else() 35 | find_library(GLOG_LIBRARY glog 36 | PATHS ${GLOG_ROOT_DIR} 37 | PATH_SUFFIXES lib lib64) 38 | endif() 39 | 40 | find_package_handle_standard_args(Glog DEFAULT_MSG GLOG_INCLUDE_DIR GLOG_LIBRARY) 41 | 42 | if(GLOG_FOUND) 43 | set(GLOG_INCLUDE_DIRS ${GLOG_INCLUDE_DIR}) 44 | set(GLOG_LIBRARIES ${GLOG_LIBRARY}) 45 | message(STATUS "Found glog (include: ${GLOG_INCLUDE_DIR}, library: ${GLOG_LIBRARY})") 46 | mark_as_advanced(GLOG_ROOT_DIR GLOG_LIBRARY_RELEASE GLOG_LIBRARY_DEBUG 47 | GLOG_LIBRARY GLOG_INCLUDE_DIR) 48 | endif() 49 | -------------------------------------------------------------------------------- /include/PoseSpline/QuaternionSplineSampleError.hpp: -------------------------------------------------------------------------------- 1 | #ifndef QUATERNIONSPLINESAMPLEERROR_H 2 | #define QUATERNIONSPLINESAMPLEERROR_H 3 | 4 | #include "PoseSpline/QuaternionSpline.hpp" 5 | #include 6 | #include 7 | #include "PoseSpline/QuaternionLocalParameter.hpp" 8 | #include "PoseSpline/ErrorInterface.hpp" 9 | 10 | 11 | /* 12 | * QuaternionSplineSampleError 13 | * 14 | * A derived Ceres cost functor to compute the Quaternon errer between quaternion measurement and 15 | * the evaluation value from spline. For cubic bspline, the evaluation at time t is related to four 16 | * contor points, so this fuctor requires four paramters. 17 | * 18 | * To simplified the jacobian, we define error = /hat ominus /meas. 19 | * 20 | * @Author Pang Fumin 21 | */ 22 | 23 | class QuaternionSplineSampleError 24 | : public ceres::SizedCostFunction<3, 4, 4, 4, 4> { 25 | public: 26 | 27 | typedef Eigen::Matrix covariance_t; 28 | typedef covariance_t information_t; 29 | 30 | QuaternionSplineSampleError(double t_meas, Quaternion Q_meas); 31 | virtual ~QuaternionSplineSampleError(); 32 | 33 | virtual bool Evaluate(double const* const* parameters, 34 | double* residuals, 35 | double** jacobians) const; 36 | bool EvaluateWithMinimalJacobians(double const* const * parameters, 37 | double* residuals, 38 | double** jacobians, 39 | double** jacobiansMinimal) const; 40 | 41 | 42 | bool VerifyJacobianNumDiff(double const* const* parameters); 43 | private: 44 | 45 | double t_meas_; 46 | Quaternion Q_Meas_; 47 | mutable information_t information_; ///< The information matrix for this error term. 48 | mutable information_t squareRootInformation_; ///< The square root information matrix for this error term. 49 | }; 50 | 51 | #endif -------------------------------------------------------------------------------- /include/PoseSpline/VectorSplineSampleAutoError.hpp: -------------------------------------------------------------------------------- 1 | #ifndef VECTORSPLINESAMPLEAUTOERROR_H 2 | #define VECTORSPLINESAMPLEAUTOERROR_H 3 | 4 | #include "PoseSpline/QuaternionSpline.hpp" 5 | #include 6 | #include 7 | #include "PoseSpline/QuaternionLocalParameter.hpp" 8 | #include "PoseSpline/ErrorInterface.hpp" 9 | #include "PoseSpline/QuaternionSplineUtility.hpp" 10 | 11 | 12 | class VectorSplineSampleAutoError { 13 | public: 14 | typedef Eigen::Matrix covariance_t; 15 | typedef covariance_t information_t; 16 | 17 | VectorSplineSampleAutoError(const double& t_meas, const Eigen::Vector3d& V_meas); 18 | virtual ~VectorSplineSampleAutoError(); 19 | 20 | 21 | 22 | template 23 | bool operator()(const T* const params0, 24 | const T* const params1, 25 | const T* const params2, 26 | const T* const params3, 27 | T* residual) const { 28 | 29 | Eigen::Map> V0(params0); 30 | Eigen::Map> V1(params1); 31 | Eigen::Map> V2(params2); 32 | Eigen::Map> V3(params3); 33 | 34 | T Beta1 = QSUtility::beta1(T(t_meas_)); 35 | T Beta2 = QSUtility::beta2(T(t_meas_)); 36 | T Beta3 = QSUtility::beta3(T(t_meas_)); 37 | 38 | // define residual 39 | // For simplity, we define error = /hat - meas. 40 | Eigen::Matrix V_hat = V0 + Beta1*(V1 - V0) + Beta2*(V2 - V1) + Beta3*(V3 - V2); 41 | 42 | Eigen::Map> error(residual); 43 | 44 | //squareRootInformation_ = Eigen::Matrix3d::Identity(); 45 | error = V_hat - V_Meas_.cast(); 46 | 47 | return true; 48 | } 49 | private: 50 | 51 | double t_meas_; 52 | Eigen::Vector3d V_Meas_; 53 | mutable information_t information_; ///< The information matrix for this error term. 54 | mutable information_t squareRootInformation_; ///< The square root information matrix for this error term. 55 | }; 56 | 57 | #endif -------------------------------------------------------------------------------- /include/extern/project_error.h: -------------------------------------------------------------------------------- 1 | 2 | #ifndef PROJECT_FACTOR_H 3 | #define PROJECT_FACTOR_H 4 | #include 5 | #include 6 | #include "ceres/ceres.h" 7 | #include 8 | #include 9 | 10 | /** 11 | * 12 | */ 13 | class ProjectError:public ceres::SizedCostFunction<2, /* num of residual */ 14 | 7, /* parameter of pose */ 15 | 3>{ 16 | 17 | public: 18 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW 19 | /// \brief The base in ceres we derive from 20 | typedef ::ceres::SizedCostFunction<2, 7, 7> base_t; 21 | 22 | /// \brief The number of residuals 23 | static const int kNumResiduals = 2; 24 | 25 | /// \brief The type of the covariance. 26 | typedef Eigen::Matrix covariance_t; 27 | 28 | /// \brief The type of the information (same matrix dimension as covariance). 29 | typedef covariance_t information_t; 30 | 31 | /// \brief The type of hte overall Jacobian. 32 | typedef Eigen::Matrix jacobian_t; 33 | 34 | /// \brief The type of the Jacobian w.r.t. poses -- 35 | /// \warning This is w.r.t. minimal tangential space coordinates... 36 | typedef Eigen::Matrix jacobian0_t; 37 | 38 | ProjectError() = delete; 39 | ProjectError(const Eigen::Vector3d& uv_C0); 40 | 41 | /// \brief Trivial destructor. 42 | virtual ~ProjectError() {} 43 | 44 | virtual bool Evaluate(double const *const *parameters, double *residuals, 45 | double **jacobians) const; 46 | 47 | bool EvaluateWithMinimalJacobians(double const *const *parameters, 48 | double *residuals, 49 | double **jacobians, 50 | double **jacobiansMinimal) const; 51 | 52 | private: 53 | 54 | Eigen::Vector3d C0uv; 55 | 56 | // information matrix and its square root 57 | mutable information_t information_; ///< The information matrix for this error term. 58 | mutable information_t squareRootInformation_; ///< The square root information matrix for this error term. 59 | }; 60 | 61 | #endif 62 | 63 | 64 | /* 65 | * 66 | */ -------------------------------------------------------------------------------- /include/PoseSpline/maplab/quaternion_param_jpl.h: -------------------------------------------------------------------------------- 1 | #ifndef CERES_ERROR_TERMS_PARAMETERIZATION_QUATERNION_PARAM_JPL_H_ 2 | #define CERES_ERROR_TERMS_PARAMETERIZATION_QUATERNION_PARAM_JPL_H_ 3 | 4 | #include 5 | #include 6 | 7 | namespace ceres_error_terms { 8 | 9 | class JplQuaternionParameterization : public ceres::LocalParameterization { 10 | public: 11 | virtual ~JplQuaternionParameterization() {} 12 | virtual bool Plus( 13 | const double* x, const double* delta, double* x_plus_delta) const; 14 | virtual bool ComputeJacobian(const double* x, double* jacobian) const; 15 | virtual int GlobalSize() const { 16 | return 4; 17 | } 18 | virtual int LocalSize() const { 19 | return 3; 20 | } 21 | 22 | template 23 | static Eigen::Matrix quatRightComp( const Eigen::Matrix q ) 24 | { 25 | // [ q3, -q2, q1, q0] 26 | // [ q2, q3, -q0, q1] 27 | // [ -q1, q0, q3, q2] 28 | // [ -q0, -q1, -q2, q3] 29 | 30 | Eigen::Matrix Q; 31 | Q(0,0) = q[3]; Q(0,1) = -q[2]; Q(0,2) = q[1]; Q(0,3) = q[0]; 32 | Q(1,0) = q[2]; Q(1,1) = q[3]; Q(1,2) = -q[0]; Q(1,3) = q[1]; 33 | Q(2,0) = -q[1]; Q(2,1) = q[0]; Q(2,2) = q[3]; Q(2,3) = q[2]; 34 | Q(3,0) = -q[0]; Q(3,1) = -q[1]; Q(3,2) = -q[2]; Q(3,3) = q[3]; 35 | 36 | return Q; 37 | } 38 | 39 | 40 | template 41 | static bool liftJacobian(const T* x, T* jacobian) { 42 | 43 | Eigen::Map > J_lift(jacobian); 44 | 45 | Eigen::Matrix q_inv(-x[0],-x[1],-x[2],x[3]); 46 | Eigen::Matrix Jq_pinv; 47 | Jq_pinv.setZero(); 48 | Jq_pinv.template topLeftCorner<3,3>() = Eigen::Matrix::Identity() * T(2.0); 49 | J_lift = Jq_pinv * quatRightComp(q_inv); 50 | 51 | return true; 52 | } 53 | // Additional interface 54 | bool ComputeLiftJacobian(const double* x, double* jacobian) const { 55 | liftJacobian(x,jacobian); 56 | return true; 57 | } 58 | 59 | }; 60 | 61 | 62 | 63 | } // namespace ceres_error_terms 64 | 65 | #endif // CERES_ERROR_TERMS_PARAMETERIZATION_QUATERNION_PARAM_JPL_H_ 66 | -------------------------------------------------------------------------------- /include/extern/pinhole_project_error.h: -------------------------------------------------------------------------------- 1 | 2 | #ifndef DEPTH_PROJECT_FACTOR_H 3 | #define DEPTH_PROJECT_FACTOR_H 4 | #include 5 | #include 6 | #include "ceres/ceres.h" 7 | #include 8 | #include 9 | 10 | /** 11 | * 12 | */ 13 | class PinholeProjectError:public ceres::SizedCostFunction<2, /* num of residual */ 14 | 7, /* parameter of pose */ 15 | 7, /* parameter of pose */ 16 | 1>{ 17 | 18 | public: 19 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW 20 | 21 | /// \brief The number of residuals 22 | static const int kNumResiduals = 2; 23 | 24 | /// \brief The type of the covariance. 25 | typedef Eigen::Matrix covariance_t; 26 | 27 | /// \brief The type of the information (same matrix dimension as covariance). 28 | typedef covariance_t information_t; 29 | 30 | /// \brief The type of hte overall Jacobian. 31 | typedef Eigen::Matrix jacobian_t; 32 | 33 | /// \brief The type of the Jacobian w.r.t. poses -- 34 | /// \warning This is w.r.t. minimal tangential space coordinates... 35 | typedef Eigen::Matrix jacobian0_t; 36 | 37 | PinholeProjectError() = delete; 38 | PinholeProjectError(const Eigen::Vector3d& uv_C0, 39 | const Eigen::Vector3d& uv_C1, 40 | const Eigen::Isometry3d _T_IC); 41 | 42 | /// \brief Trivial destructor. 43 | virtual ~PinholeProjectError() {} 44 | 45 | virtual bool Evaluate(double const *const *parameters, double *residuals, 46 | double **jacobians) const; 47 | 48 | bool EvaluateWithMinimalJacobians(double const *const *parameters, 49 | double *residuals, 50 | double **jacobians, 51 | double **jacobiansMinimal) const; 52 | 53 | private: 54 | 55 | Eigen::Vector3d C0uv; 56 | Eigen::Vector3d C1uv; 57 | Eigen::Vector3d t_IC; 58 | Eigen::Matrix3d R_IC; 59 | // information matrix and its square root 60 | mutable information_t information_; ///< The information matrix for this error term. 61 | mutable information_t squareRootInformation_; ///< The square root information matrix for this error term. 62 | }; 63 | 64 | #endif 65 | 66 | 67 | /* 68 | * 69 | */ -------------------------------------------------------------------------------- /include/PoseSpline/QuaternionSplineSampleAutoError.hpp: -------------------------------------------------------------------------------- 1 | #ifndef QUATERNIONSPLINESAMPLEAUTOERROR_H 2 | #define QUATERNIONSPLINESAMPLEAUTOERROR_H 3 | 4 | #include "PoseSpline/QuaternionSpline.hpp" 5 | #include 6 | #include 7 | #include "PoseSpline/QuaternionLocalParameter.hpp" 8 | #include "PoseSpline/ErrorInterface.hpp" 9 | 10 | 11 | 12 | class QuaternionSplineSampleAutoError { 13 | public: 14 | 15 | typedef Eigen::Matrix covariance_t; 16 | typedef covariance_t information_t; 17 | 18 | QuaternionSplineSampleAutoError(double t_meas, 19 | Quaternion Q_meas): 20 | t_meas_(t_meas),Q_Meas_(Q_meas){ 21 | 22 | }; 23 | 24 | virtual ~QuaternionSplineSampleAutoError(){}; 25 | 26 | template 27 | bool operator()(const T* const params0, 28 | const T* const params1, 29 | const T* const params2, 30 | const T* const params3, 31 | T* residual) const { 32 | Eigen::Map> Q0(params0); 33 | Eigen::Map> Q1(params0); 34 | Eigen::Map> Q2(params0); 35 | Eigen::Map> Q3(params0); 36 | 37 | Eigen::Map> error(residual); 38 | 39 | 40 | T Beta1 = QSUtility::beta1((T)t_meas_); 41 | T Beta2 = QSUtility::beta2((T)t_meas_); 42 | T Beta3 = QSUtility::beta3((T)t_meas_); 43 | 44 | Eigen::Matrix phi1 = QSUtility::Phi(Q0,Q1); 45 | Eigen::Matrix phi2 = QSUtility::Phi(Q1,Q2); 46 | Eigen::Matrix phi3 = QSUtility::Phi(Q2,Q3); 47 | 48 | Eigen::Matrix r_1 = QSUtility::r(Beta1,phi1); 49 | Eigen::Matrix r_2 = QSUtility::r(Beta2,phi2); 50 | Eigen::Matrix r_3 = QSUtility::r(Beta3,phi3); 51 | 52 | // define residual 53 | // For simplity, we define error = /hat - meas. 54 | // delte_Q = Q_hat*inv(Q_meas) = (inv(Q_meas))oplus Q_hat 55 | Eigen::Matrix Q_hat = quatLeftComp(Q0)*quatLeftComp(r_1)*quatLeftComp(r_2)*r_3; 56 | Eigen::Matrix dQ = quatLeftComp(Q_hat)*quatInv(Q_Meas_.cast()); 57 | error = T(2) * dQ.topLeftCorner(3,1); 58 | return true; 59 | } 60 | 61 | 62 | 63 | private: 64 | 65 | double t_meas_; 66 | Quaternion Q_Meas_; 67 | mutable information_t information_; ///< The information matrix for this error term. 68 | mutable information_t squareRootInformation_; ///< The square root information matrix for this error term. 69 | }; 70 | 71 | #endif -------------------------------------------------------------------------------- /include/PoseSpline/QuaternionLocalParameter.hpp: -------------------------------------------------------------------------------- 1 | #ifndef QUATERNIONLOCALPARAMETER_H 2 | #define QUATERNIONLOCALPARAMETER_H 3 | 4 | 5 | #include 6 | #include 7 | #include "Quaternion.hpp" 8 | class QuaternionLocalParameter: public ceres::LocalParameterization{ 9 | 10 | 11 | public: 12 | virtual ~QuaternionLocalParameter() { 13 | } 14 | 15 | virtual bool Plus(const double *x, const double *delta, double *x_plus_delta) const { 16 | Eigen::Map Q_(x); // Q bar 17 | Eigen::Map delta_phi(delta); 18 | 19 | Quaternion dq; 20 | dq << 0.5*delta_phi, 1.0; 21 | 22 | Eigen::Map Q_plus(x_plus_delta); 23 | Q_plus = quatLeftComp(dq)*Q_; 24 | 25 | Q_plus = Q_plus/Q_plus.norm(); // normilize 26 | 27 | return true; 28 | } 29 | virtual bool ComputeJacobian(const double *x, double *jacobian) const { 30 | plusJacobian(x,jacobian); 31 | return true; 32 | } 33 | virtual int GlobalSize() const { return 4; }; 34 | virtual int LocalSize() const { return 3; }; 35 | 36 | 37 | static bool plusJacobian(const double* x,double* jacobian) { 38 | Eigen::Map Q_(x); 39 | Eigen::Map> J(jacobian); 40 | 41 | /* 42 | Eigen::Matrix m; 43 | m.setZero(); 44 | m.topLeftCorner(3,3).setIdentity(); 45 | J = quatRightComp(Q_)*0.5*m; 46 | */ 47 | 48 | /// more effient 49 | J.setZero(); 50 | J(0, 0) = Q_(3); 51 | J(0, 1) = -Q_(2); 52 | J(0, 2) = Q_(1); 53 | J(1, 0) = Q_(2); 54 | J(1, 1) = Q_(3); 55 | J(1, 2) = -Q_(0); 56 | J(2, 0) = -Q_(1); 57 | J(2, 1) = Q_(0); 58 | J(2, 2) = Q_(3); 59 | J(3, 0) = -Q_(0); 60 | J(3, 1) = -Q_(1); 61 | J(3, 2) = -Q_(2); 62 | J *= 0.5; 63 | return true; 64 | 65 | } 66 | template 67 | static bool liftJacobian(const T* x, T* jacobian) { 68 | 69 | Eigen::Map > J_lift(jacobian); 70 | 71 | QuaternionTemplate q_inv(-x[0],-x[1],-x[2],x[3]); 72 | Eigen::Matrix Jq_pinv; 73 | Jq_pinv.setZero(); 74 | Jq_pinv.template topLeftCorner<3,3>() = Eigen::Matrix::Identity() * T(2.0); 75 | J_lift = Jq_pinv*quatRightComp(q_inv); 76 | 77 | return true; 78 | } 79 | // Additional interface 80 | bool ComputeLiftJacobian(const double* x, double* jacobian) const { 81 | liftJacobian(x,jacobian); 82 | return true; 83 | } 84 | 85 | }; 86 | 87 | 88 | #endif -------------------------------------------------------------------------------- /src/maplab/quaternion_param_jpl.cpp: -------------------------------------------------------------------------------- 1 | #include "PoseSpline/maplab/quaternion_param_jpl.h" 2 | 3 | // #include 4 | #include "PoseSpline/maplab/quaternion-math.h" 5 | 6 | namespace ceres_error_terms { 7 | namespace { 8 | inline void get_dQuaternionJpl_dTheta( 9 | const double* q_ptr, double* jacobian_row_major) { 10 | CHECK_NOTNULL(q_ptr); 11 | CHECK_NOTNULL(jacobian_row_major); 12 | 13 | const Eigen::Map> q(q_ptr); 14 | Eigen::Matrix temp; 15 | if (q(3) < 0) { 16 | temp = -q; 17 | } else { 18 | temp = q; 19 | } 20 | CHECK_GE(temp(3), 0.); 21 | 22 | Eigen::Map> jacobian( 23 | jacobian_row_major); 24 | jacobian.setZero(); 25 | jacobian(0, 0) = temp(3); 26 | jacobian(0, 1) = -temp(2); 27 | jacobian(0, 2) = temp(1); 28 | jacobian(1, 0) = temp(2); 29 | jacobian(1, 1) = temp(3); 30 | jacobian(1, 2) = -temp(0); 31 | jacobian(2, 0) = -temp(1); 32 | jacobian(2, 1) = temp(0); 33 | jacobian(2, 2) = temp(3); 34 | jacobian(3, 0) = -temp(0); 35 | jacobian(3, 1) = -temp(1); 36 | jacobian(3, 2) = -temp(2); 37 | jacobian *= 0.5; 38 | } 39 | } // namespace 40 | 41 | bool JplQuaternionParameterization::Plus( 42 | const double* x, const double* delta, double* x_plus_delta) const { 43 | CHECK_NOTNULL(x); 44 | CHECK_NOTNULL(delta); 45 | CHECK_NOTNULL(x_plus_delta); 46 | 47 | Eigen::Map rot_vector_delta(delta); 48 | Eigen::Map q(x); 49 | Eigen::Map q_product(x_plus_delta); 50 | CHECK_GE(q(3), 0.); 51 | 52 | double square_norm_delta = rot_vector_delta.squaredNorm(); 53 | // 0.262rad is ~15deg which keeps the small angle approximation error limited 54 | // to about 1%. 55 | static constexpr double kSmallAngleApproxThresholdDoubleSquared = 56 | (0.262 * 2) * (0.262 * 2); 57 | 58 | Eigen::Vector4d q_delta; 59 | if (square_norm_delta < kSmallAngleApproxThresholdDoubleSquared) { 60 | // The delta theta norm is below the threshold so we can use the small 61 | // angle approximation. 62 | q_delta << 0.5 * rot_vector_delta, 1.0; 63 | q_delta.normalize(); 64 | } else { 65 | const double norm_delta_half = 0.5 * sqrt(square_norm_delta); 66 | q_delta.head<3>() = 67 | 0.5 * rot_vector_delta / norm_delta_half * std::sin(norm_delta_half); 68 | q_delta(3) = std::cos(norm_delta_half); 69 | } 70 | common::positiveQuaternionProductJPL(q_delta, q, q_product); 71 | CHECK_GE(q_product(3), 0.); 72 | 73 | return true; 74 | } 75 | 76 | bool JplQuaternionParameterization::ComputeJacobian( 77 | const double* quat, double* jacobian_row_major) const { 78 | CHECK_NOTNULL(quat); 79 | CHECK_NOTNULL(jacobian_row_major); 80 | get_dQuaternionJpl_dTheta(quat, jacobian_row_major); 81 | return true; 82 | } 83 | 84 | 85 | 86 | } // namespace ceres_error_terms 87 | -------------------------------------------------------------------------------- /include/PoseSpline/PoseSplineUtility.hpp: -------------------------------------------------------------------------------- 1 | #ifndef POSESPLINEUTILITY_H 2 | #define POSESPLINEUTILITY_H 3 | 4 | 5 | #include "PoseSpline/QuaternionSplineUtility.hpp" 6 | #include "PoseSpline/Pose.hpp" 7 | 8 | class PSUtility { 9 | public: 10 | static Pose EvaluatePS(double u, const Pose& P0, const Pose& P1, 11 | const Pose& P2, const Pose& P3); 12 | static Eigen::Vector3d EvaluateLinearVelocity(double u, double dt, 13 | const Eigen::Vector3d& V0, 14 | const Eigen::Vector3d& V1, 15 | const Eigen::Vector3d& V2, 16 | const Eigen::Vector3d& V3); 17 | 18 | static Eigen::Vector3d EvaluatePosition(double u, 19 | const Eigen::Vector3d& V0, 20 | const Eigen::Vector3d& V1, 21 | const Eigen::Vector3d& V2, 22 | const Eigen::Vector3d& V3); 23 | static Eigen::Vector3d EvaluateLinearAccelerate(double u, double dt, 24 | const Pose& P0, const Pose& P1, 25 | const Pose& P2, const Pose& P3, 26 | const Eigen::Vector3d& gravity); 27 | }; 28 | 29 | class PoseSplineEvaluation { 30 | public: 31 | typedef Eigen::Matrix TranslationJacobian; 32 | typedef Eigen::Matrix RotationJacobian; 33 | Pose operator() (double u, const Pose& P0, const Pose& P1, 34 | const Pose& P2, const Pose& P3); 35 | template 36 | TranslationJacobian getTranslationJacobian () { 37 | if (D == 0){ 38 | return JacobianTrans0; 39 | }else if(D == 1){ 40 | return JacobianTrans1; 41 | }else if(D == 2){ 42 | return JacobianTrans2; 43 | }else { 44 | return JacobianTrans3; 45 | } 46 | } 47 | template 48 | RotationJacobian getRotationJacobianMinimal () { 49 | if (D == 0){ 50 | return JacobianRotate0; 51 | }else if(D == 1){ 52 | return JacobianRotate1; 53 | }else if(D == 2){ 54 | return JacobianRotate2; 55 | }else { 56 | return JacobianRotate3; 57 | } 58 | } 59 | 60 | inline Quaternion getRotation() { 61 | return Q; 62 | } 63 | 64 | inline Eigen::Vector3d getTranslation() { 65 | return t; 66 | } 67 | 68 | private: 69 | Quaternion Q; 70 | Eigen::Vector3d t; 71 | TranslationJacobian JacobianTrans0,JacobianTrans1,JacobianTrans2,JacobianTrans3; 72 | RotationJacobian JacobianRotate0,JacobianRotate1,JacobianRotate2,JacobianRotate3; 73 | }; 74 | 75 | #endif 76 | 77 | -------------------------------------------------------------------------------- /src/extern/spline_projection_error3.cpp: -------------------------------------------------------------------------------- 1 | #include "extern/spline_projection_error3.h" 2 | #include "PoseSpline/QuaternionLocalParameter.hpp" 3 | #include "PoseSpline/PoseLocalParameter.hpp" 4 | 5 | SplineProjectError3::SplineProjectError3(const SplineProjectFunctor3& functor): 6 | functor_(functor){} 7 | 8 | 9 | bool SplineProjectError3::Evaluate(double const *const *parameters, 10 | double *residuals, 11 | double **jacobians) const { 12 | return EvaluateWithMinimalJacobians(parameters, 13 | residuals, 14 | jacobians, NULL); 15 | } 16 | 17 | 18 | bool SplineProjectError3::EvaluateWithMinimalJacobians(double const *const *parameters, 19 | double *residuals, 20 | double **jacobians, 21 | double **jacobiansMinimal) const { 22 | 23 | if (!jacobians) { 24 | return ceres::internal::VariadicEvaluate< 25 | SplineProjectFunctor3, double, 7, 7, 7, 7, 7, 7, 7, 1,1,0> 26 | ::Call(functor_, parameters, residuals); 27 | } 28 | 29 | 30 | 31 | 32 | bool success = ceres::internal::AutoDiff::Differentiate( 34 | functor_, 35 | parameters, 36 | 2, 37 | residuals, 38 | jacobians); 39 | 40 | if (success && jacobiansMinimal!= NULL) { 41 | Pose T0(parameters[0]); 42 | Pose T1(parameters[1]); 43 | Pose T2(parameters[2]); 44 | Pose T3(parameters[3]); 45 | Pose T4(parameters[4]); 46 | Pose T5(parameters[5]); 47 | Pose T6(parameters[6]); 48 | for (int i = 0; i < 7 && jacobiansMinimal[i] != NULL; i++) { 49 | Eigen::Map> J_minimal_map(jacobiansMinimal[i]); 50 | Eigen::Matrix J_plus; 51 | PoseLocalParameter::plusJacobian(Pose(parameters[i]).parameterPtr(), J_plus.data()); 52 | Eigen::Map> J_map(jacobians[i]); 53 | J_minimal_map = J_map * J_plus; 54 | 55 | } 56 | 57 | if( jacobiansMinimal[7] != NULL){ 58 | Eigen::Map> J7_minimal_map(jacobiansMinimal[7]); 59 | Eigen::Map> J7_map(jacobians[7]); 60 | J7_minimal_map = J7_map; 61 | } 62 | 63 | if( jacobiansMinimal[8] != NULL){ 64 | Eigen::Map> J_minimal_map(jacobiansMinimal[8]); 65 | Eigen::Map> J_map(jacobians[8]); 66 | J_minimal_map = J_map; 67 | } 68 | 69 | 70 | } 71 | 72 | return true; 73 | } -------------------------------------------------------------------------------- /include/PoseSpline/PoseLocalParameter.hpp: -------------------------------------------------------------------------------- 1 | #ifndef POSELOCALPARAMETER_H 2 | #define POSELOCALPARAMETER_H 3 | 4 | 5 | #include 6 | #include 7 | #include "PoseSpline/Pose.hpp" 8 | 9 | 10 | /* 11 | * PoseLocalParameter 12 | * Hers we define the pose representaion: first translate , then rotation quaternion 13 | */ 14 | class PoseLocalParameter: public ceres::LocalParameterization{ 15 | 16 | 17 | public: 18 | virtual ~PoseLocalParameter() {}; 19 | virtual bool Plus(const double *x, const double *delta, double *x_plus_delta) const { 20 | return plus(x, delta, x_plus_delta); 21 | } 22 | virtual bool ComputeJacobian(const double *x, double *jacobian) const{ 23 | plusJacobian(x,jacobian); 24 | return true; 25 | } 26 | virtual int GlobalSize() const { return 7; }; 27 | virtual int LocalSize() const { return 6; }; 28 | 29 | static bool plus(const double* x, const double* delta, double* x_plus_delta) { 30 | Eigen::Map > delta_(delta); 31 | 32 | // transform to okvis::kinematics framework 33 | Pose T(Eigen::Vector3d(x[0], x[1], x[2]), 34 | Quaternion ( x[3], x[4], x[5], x[6])); 35 | 36 | // call oplus operator in okvis::kinematis 37 | T.oplus(delta_); 38 | 39 | // copy back 40 | const Eigen::Vector3d r = T.r(); 41 | x_plus_delta[0] = r[0]; 42 | x_plus_delta[1] = r[1]; 43 | x_plus_delta[2] = r[2]; 44 | const Eigen::Vector4d q = T.q(); 45 | x_plus_delta[3] = q[0]; 46 | x_plus_delta[4] = q[1]; 47 | x_plus_delta[5] = q[2]; 48 | x_plus_delta[6] = q[3]; 49 | 50 | //OKVIS_ASSERT_TRUE_DBG(std::runtime_error, T.q().norm()-1.0<1e-15, "damn."); 51 | 52 | return true; 53 | } 54 | 55 | 56 | // Extent interface 57 | bool ComputeLiftJacobian(const double* x, double* jacobian) const { 58 | liftJacobian(x,jacobian); 59 | return true; 60 | 61 | } 62 | static bool liftJacobian(const double* x,double* jacobian) { 63 | Eigen::Map > J_lift(jacobian); 64 | 65 | J_lift.setIdentity(); 66 | Quaternion q_inv(-x[3],-x[4],-x[5],x[6]); 67 | Eigen::Matrix Jq_pinv; 68 | Jq_pinv.setZero(); 69 | Jq_pinv.topLeftCorner<3,3>() = Eigen::Matrix3d::Identity() * 2.0; 70 | J_lift.bottomRightCorner(3,4) = Jq_pinv*quatRightComp(q_inv); 71 | 72 | return true; 73 | } 74 | static bool plusJacobian(const double* x,double* jacobian) { 75 | Eigen::Map> J(jacobian); 76 | J.setIdentity(); 77 | Eigen::Map Q_(x+3); 78 | 79 | Eigen::Matrix m; 80 | m.setZero(); 81 | m.topLeftCorner(3,3).setIdentity(); 82 | J.bottomRightCorner(4,3) = quatRightComp(Q_)*0.5*m; 83 | return true; 84 | } 85 | }; 86 | 87 | 88 | #endif -------------------------------------------------------------------------------- /src/extern/spline_projection_error4.cpp: -------------------------------------------------------------------------------- 1 | #include "extern/spline_projection_error4.h" 2 | #include "PoseSpline/QuaternionLocalParameter.hpp" 3 | #include "PoseSpline/PoseLocalParameter.hpp" 4 | 5 | SplineProjectError4::SplineProjectError4(const SplineProjectFunctor4& functor): 6 | functor_(functor){} 7 | 8 | 9 | bool SplineProjectError4::Evaluate(double const *const *parameters, 10 | double *residuals, 11 | double **jacobians) const { 12 | return EvaluateWithMinimalJacobians(parameters, 13 | residuals, 14 | jacobians, NULL); 15 | } 16 | 17 | 18 | bool SplineProjectError4::EvaluateWithMinimalJacobians(double const *const *parameters, 19 | double *residuals, 20 | double **jacobians, 21 | double **jacobiansMinimal) const { 22 | 23 | if (!jacobians) { 24 | return ceres::internal::VariadicEvaluate< 25 | SplineProjectFunctor4, double, 7, 7, 7, 7, 7, 7, 7, 7, 1,1> 26 | ::Call(functor_, parameters, residuals); 27 | } 28 | 29 | 30 | 31 | 32 | bool success = ceres::internal::AutoDiff::Differentiate( 34 | functor_, 35 | parameters, 36 | 2, 37 | residuals, 38 | jacobians); 39 | 40 | if (success && jacobiansMinimal!= NULL) { 41 | Pose T0(parameters[0]); 42 | Pose T1(parameters[1]); 43 | Pose T2(parameters[2]); 44 | Pose T3(parameters[3]); 45 | Pose T4(parameters[4]); 46 | Pose T5(parameters[5]); 47 | Pose T6(parameters[6]); 48 | Pose T7(parameters[7]); 49 | for (int i = 0; i < 8 && jacobiansMinimal[i] != NULL; i++) { 50 | Eigen::Map> J_minimal_map(jacobiansMinimal[i]); 51 | Eigen::Matrix J_plus; 52 | PoseLocalParameter::plusJacobian(Pose(parameters[i]).parameterPtr(), J_plus.data()); 53 | Eigen::Map> J_map(jacobians[i]); 54 | J_minimal_map = J_map * J_plus; 55 | 56 | } 57 | 58 | if( jacobiansMinimal[8] != NULL){ 59 | Eigen::Map> J8_minimal_map(jacobiansMinimal[8]); 60 | Eigen::Map> J8_map(jacobians[8]); 61 | J8_minimal_map = J8_map; 62 | } 63 | 64 | if( jacobiansMinimal[9] != NULL){ 65 | Eigen::Map> J_minimal_map(jacobiansMinimal[9]); 66 | Eigen::Map> J_map(jacobians[9]); 67 | J_minimal_map = J_map; 68 | } 69 | 70 | 71 | } 72 | 73 | return true; 74 | } -------------------------------------------------------------------------------- /test/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | # Make test executable 2 | 3 | 4 | add_executable(QuaternionCeresTest QuaternionCeresTest.cpp) 5 | target_link_libraries(QuaternionCeresTest 6 | PoseSpline) 7 | 8 | 9 | add_executable(QuaternionSplineCeresTest QuaternionSplineCeresTest.cpp) 10 | target_link_libraries(QuaternionSplineCeresTest 11 | PoseSpline) 12 | 13 | add_executable(QuaternionOmegaCeresTest QuaternionOmegaCeresTest.cpp) 14 | target_link_libraries(QuaternionOmegaCeresTest 15 | PoseSpline) 16 | 17 | add_executable(VectorSplineVelocityCeresTest VectorSplineVelocityCeresTest.cpp) 18 | target_link_libraries(VectorSplineVelocityCeresTest 19 | PoseSpline) 20 | 21 | add_executable(VectorSplineCeresTest VectorSplineCeresTest.cpp) 22 | target_link_libraries(VectorSplineCeresTest 23 | PoseSpline) 24 | 25 | add_executable(testCamera testCamera.cpp) 26 | target_link_libraries(testCamera 27 | PoseSpline) 28 | 29 | 30 | 31 | 32 | 33 | #add_executable(AngularVelocitySampleTest AngularVelocitySampleTest.cpp) 34 | #target_link_libraries(AngularVelocitySampleTest 35 | # PoseSpline) 36 | # 37 | 38 | add_executable(QuaternionSplineTest QuaternionSplineTest.cpp) 39 | target_link_libraries(QuaternionSplineTest 40 | PoseSpline) 41 | 42 | add_executable(PoseSplineTest PoseSplineTest.cpp) 43 | target_link_libraries(PoseSplineTest 44 | PoseSpline) 45 | 46 | 47 | 48 | add_executable(VectorSpaceSplineTest VectorSpaceSplineTest.cpp) 49 | target_link_libraries(VectorSpaceSplineTest 50 | PoseSpline) 51 | 52 | add_executable(UtilityTest UtilityTest.cpp) 53 | target_link_libraries(UtilityTest 54 | PoseSpline) 55 | 56 | 57 | 58 | add_executable(testPinholeProjectFactor testPinholeProjectFactor.cpp) 59 | target_link_libraries(testPinholeProjectFactor 60 | PoseSpline) 61 | 62 | add_executable(testProjectError testProjectError.cpp) 63 | target_link_libraries(testProjectError 64 | PoseSpline) 65 | 66 | add_executable(testPoseSplineSampleError testPoseSplineSampleError.cpp) 67 | target_link_libraries(testPoseSplineSampleError 68 | PoseSpline) 69 | 70 | #add_executable(testLinearAccelerateSampleError testLinearAccelerateSampleError.cpp) 71 | #target_link_libraries(testLinearAccelerateSampleError 72 | # PoseSpline) 73 | 74 | 75 | add_executable(testSplineRotateError testSplineRotateError.cpp) 76 | target_link_libraries(testSplineRotateError 77 | PoseSpline) 78 | 79 | 80 | 81 | 82 | 83 | # add_executable(test_main 84 | # test_main.cpp PoseTest.cpp) 85 | # target_link_libraries(test_main PoseSpline Catch) 86 | 87 | # gtest 88 | #enable_testing() 89 | # 90 | #include_directories(include 91 | # ${OpenCV_INCLUDE_DIRS}) 92 | #set(FRAME_TEST_SOURCES 93 | # #frame/TestFrame.cpp 94 | # frame/TestMultiFrame.cpp 95 | # frame/TestNCameraSystem.cpp 96 | # frame/TestPinholeCamera.cpp) 97 | # 98 | #add_executable(frame_test ${FRAME_TEST_SOURCES} ./gtest/gtest-all.cc) 99 | #target_link_libraries(frame_test pthread cv util ${OpenCV_LIBRARIES}) 100 | #set_target_properties(frame_test PROPERTIES COMPILE_FLAGS "-std=c++11 -Wno-deprecated-register") 101 | # 102 | #add_test(NAME frame_test COMMAND frame_test --gtest_color=yes) 103 | -------------------------------------------------------------------------------- /src/OmegaExtrinsicTemperalError.cpp: -------------------------------------------------------------------------------- 1 | #include "PoseSpline/OmegaExtrinsicTemperalError.hpp" 2 | #include "PoseSpline/QuaternionSplineUtility.hpp" 3 | 4 | 5 | OmegaExtrinsicTemperalError::OmegaExtrinsicTemperalError(): 6 | omega_meas_(Eigen::Matrix::Zero()), 7 | Q_cw_(unitQuat()), 8 | dotQ_cw_(unitQuat()), 9 | dotdotQ_cw_(unitQuat()){ 10 | 11 | 12 | } 13 | 14 | 15 | OmegaExtrinsicTemperalError::OmegaExtrinsicTemperalError(const Eigen::Vector3d& omega_meas, 16 | const Quaternion& Q_cw, 17 | const Quaternion& dotQ_cw, 18 | const Quaternion& dotdotQ_cw): 19 | omega_meas_(omega_meas), 20 | Q_cw_(Q_cw), 21 | dotQ_cw_(dotQ_cw), 22 | dotdotQ_cw_(dotdotQ_cw){ 23 | 24 | 25 | } 26 | 27 | OmegaExtrinsicTemperalError::~OmegaExtrinsicTemperalError(){ 28 | 29 | } 30 | 31 | bool OmegaExtrinsicTemperalError::Evaluate(double const* const* parameters, 32 | double* residuals, 33 | double** jacobians) const{ 34 | 35 | return EvaluateWithMinimalJacobians(parameters,residuals,jacobians,NULL); 36 | 37 | } 38 | 39 | //todo: finish it 40 | bool OmegaExtrinsicTemperalError::EvaluateWithMinimalJacobians(double const* const * parameters, 41 | double* residuals, 42 | double** jacobians, 43 | double** jacobiansMinimal) const{ 44 | Eigen::Map param_ext_Q_cs(jacobians[0]); 45 | double param_delta_t = jacobians[1][0]; 46 | 47 | Quaternion dotQinvQ = quatLeftComp(dotQ_cw_)*quatInv(Q_cw_); 48 | Eigen::Vector4d J_dotQinvQ_t = QSUtility::Jacobian_dotQinvQ_t(Q_cw_,dotQ_cw_,dotdotQ_cw_); 49 | Quaternion dotQinvQ_linearize = dotQinvQ + J_dotQinvQ_t*param_delta_t; // linearize 50 | Quaternion invQ_cs = quatInv(param_ext_Q_cs); 51 | Quaternion invQ_cw = quatInv(Q_cw_); 52 | Eigen::Vector3d omega_hat = 2.0*(quatLeftComp(invQ_cs)*quatLeftComp(dotQinvQ_linearize)*param_ext_Q_cs).head(3); 53 | 54 | // For simplity, define error = \hat - \meas 55 | Eigen::Map error(residuals); 56 | error = omega_hat - omega_meas_; 57 | 58 | 59 | // Jacobians 60 | if(jacobians != NULL){ 61 | 62 | 63 | if(jacobians[0] != NULL){ 64 | 65 | Eigen::Map> jacobian0(jacobians[0]); 66 | Eigen::Matrix jacobian0_min; 67 | 68 | jacobian0_min = QSUtility::Jacobian_omega_extrinsicQ(Q_cw_,dotQ_cw_,param_ext_Q_cs); 69 | 70 | QuaternionLocalParameter localPrameter; 71 | Eigen::Matrix lift; 72 | localPrameter.ComputeLiftJacobian(parameters[0],lift.data()); 73 | jacobian0 = jacobian0_min*lift; 74 | 75 | 76 | if(jacobiansMinimal != NULL && jacobiansMinimal[0] != NULL){ 77 | 78 | } 79 | 80 | 81 | } 82 | if(jacobians[1] != NULL){ 83 | 84 | if(jacobiansMinimal != NULL && jacobiansMinimal[1] != NULL){ 85 | 86 | } 87 | 88 | } 89 | } 90 | 91 | 92 | return true; 93 | 94 | } 95 | -------------------------------------------------------------------------------- /src/QuaternionOmegaSampleError.cpp: -------------------------------------------------------------------------------- 1 | #include "PoseSpline/QuaternionOmegaSampleError.hpp" 2 | #include "PoseSpline/QuaternionLocalParameter.hpp" 3 | 4 | bool QuaternionOmegaSampleAutoError::Evaluate(double const* const* parameters, 5 | double* residuals, 6 | double** jacobians) const { 7 | 8 | return EvaluateWithMinimalJacobians(parameters, residuals,jacobians, NULL); 9 | 10 | } 11 | 12 | bool QuaternionOmegaSampleAutoError::EvaluateWithMinimalJacobians(double const* const * parameters, 13 | double* residuals, 14 | double** jacobians, 15 | double** jacobiansMinimal) const { 16 | 17 | Eigen::Map Q0(parameters[0]); 18 | Eigen::Map Q1(parameters[1]); 19 | Eigen::Map Q2(parameters[2]); 20 | Eigen::Map Q3(parameters[3]); 21 | 22 | 23 | if (!jacobians) { 24 | return ceres::internal::VariadicEvaluate< 25 | QuaternionOmegaSampleFunctor, double, 4, 4, 4, 4, 0,0,0,0,0,0> 26 | ::Call(*functor_, parameters, residuals); 27 | } 28 | bool success = ceres::internal::AutoDiff::Differentiate( 30 | *functor_, 31 | parameters, 32 | SizedCostFunction<3, 33 | 4,4,4,4>::num_residuals(), 34 | residuals, 35 | jacobians); 36 | 37 | if (success && jacobiansMinimal!= NULL) { 38 | if( jacobiansMinimal[0] != NULL){ 39 | 40 | Eigen::Map> J0_minimal_map(jacobiansMinimal[0]); 41 | Eigen::Matrix J_plus; 42 | QuaternionLocalParameter::plusJacobian(Q0.data(),J_plus.data()); 43 | Eigen::Map> J0_map(jacobians[0]); 44 | J0_minimal_map = J0_map*J_plus; 45 | } 46 | 47 | if( jacobiansMinimal[1] != NULL){ 48 | 49 | Eigen::Map> J1_minimal_map(jacobiansMinimal[1]); 50 | Eigen::Matrix J_plus; 51 | QuaternionLocalParameter::plusJacobian(Q1.data(),J_plus.data()); 52 | Eigen::Map> J1_map(jacobians[1]); 53 | J1_minimal_map = J1_map*J_plus; 54 | } 55 | 56 | if( jacobiansMinimal[2] != NULL){ 57 | 58 | Eigen::Map> J2_minimal_map(jacobiansMinimal[2]); 59 | Eigen::Matrix J_plus; 60 | QuaternionLocalParameter::plusJacobian(Q2.data(),J_plus.data()); 61 | Eigen::Map> J2_map(jacobians[2]); 62 | J2_minimal_map = J2_map*J_plus; 63 | } 64 | 65 | if( jacobiansMinimal[3] != NULL){ 66 | 67 | Eigen::Map> J3_minimal_map(jacobiansMinimal[3]); 68 | Eigen::Matrix J_plus; 69 | QuaternionLocalParameter::plusJacobian(Q3.data(),J_plus.data()); 70 | Eigen::Map> J3_map(jacobians[3]); 71 | J3_minimal_map = J3_map*J_plus; 72 | } 73 | 74 | } 75 | 76 | return success; 77 | } -------------------------------------------------------------------------------- /src/Duration.cpp: -------------------------------------------------------------------------------- 1 | /********************************************************************* 2 | * Software License Agreement (BSD License) 3 | * 4 | * Modified and adapted by 5 | * Copyright (c) 2015, Autonomous Systems Lab / ETH Zurich 6 | * 7 | * Original Copyright (c) 2008, Willow Garage, Inc. 8 | * All rights reserved. 9 | * 10 | * Redistribution and use in source and binary forms, with or without 11 | * modification, are permitted provided that the following conditions 12 | * are met: 13 | * 14 | * * Redistributions of source code must retain the above copyright 15 | * notice, this list of conditions and the following disclaimer. 16 | * * Redistributions in binary form must reproduce the above 17 | * copyright notice, this list of conditions and the following 18 | * disclaimer in the documentation and/or other materials provided 19 | * with the distribution. 20 | * * Neither the name of Willow Garage, Inc. nor the names of its 21 | * contributors may be used to endorse or promote products derived 22 | * from this software without specific prior written permission. 23 | * 24 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 25 | * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 26 | * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 27 | * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 28 | * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 29 | * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 30 | * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 31 | * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 32 | * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 33 | * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 34 | * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 35 | * POSSIBILITY OF SUCH DAMAGE. 36 | *********************************************************************/ 37 | /* 38 | * Modified: Stefan Leutenegger (s.leutenegger@imperial.ac.uk) 39 | */ 40 | 41 | /** 42 | * @file Duration.cpp 43 | * @brief This file contains osme helper function for the Duration class. 44 | * @author Willow Garage Inc. 45 | * @author Stefan Leutenegger 46 | * @author Andreas Forster 47 | */ 48 | 49 | #include 50 | #include 51 | 52 | 53 | void normalizeSecNSecSigned(int64_t& sec, int64_t& nsec) { 54 | int64_t nsec_part = nsec; 55 | int64_t sec_part = sec; 56 | 57 | while (nsec_part > 1000000000L) { 58 | nsec_part -= 1000000000L; 59 | ++sec_part; 60 | } 61 | while (nsec_part < 0) { 62 | nsec_part += 1000000000L; 63 | --sec_part; 64 | } 65 | 66 | if (sec_part < INT_MIN || sec_part > INT_MAX) 67 | throw std::runtime_error("Duration is out of dual 32-bit range"); 68 | 69 | sec = sec_part; 70 | nsec = nsec_part; 71 | } 72 | 73 | void normalizeSecNSecSigned(int32_t& sec, int32_t& nsec) { 74 | int64_t sec64 = sec; 75 | int64_t nsec64 = nsec; 76 | 77 | normalizeSecNSecSigned(sec64, nsec64); 78 | 79 | sec = (int32_t) sec64; 80 | nsec = (int32_t) nsec64; 81 | } 82 | 83 | template class DurationBase ; 84 | template class DurationBase ; 85 | -------------------------------------------------------------------------------- /include/PoseSpline/maplab/quaternion-math.h: -------------------------------------------------------------------------------- 1 | #ifndef MAPLAB_COMMON_QUATERNION_MATH_H_ 2 | #define MAPLAB_COMMON_QUATERNION_MATH_H_ 3 | 4 | #include 5 | 6 | // #include 7 | #include "geometry.h" 8 | // #include 9 | 10 | namespace common { 11 | 12 | template 13 | void toRotationMatrixJPL( 14 | const Eigen::MatrixBase& q, 15 | Eigen::Matrix* rot_matrix); 16 | 17 | template 18 | void fromRotationMatrixJPL( 19 | const Eigen::MatrixBase& rot, 20 | Eigen::Matrix* q_JPL); 21 | 22 | template 23 | void rotationVectorToQuaternionJPL( 24 | const Eigen::Matrix& A_rotation_vector, 25 | const Eigen::Matrix& q_A_Ahat_const); 26 | 27 | template 28 | void signedQuaternionProductJPL( 29 | const Eigen::MatrixBase& q1, 30 | const Eigen::MatrixBase& q2, 31 | const Eigen::MatrixBase& product_const); 32 | 33 | template 34 | void positiveQuaternionProductJPL( 35 | const Eigen::MatrixBase& q1, 36 | const Eigen::MatrixBase& q2, 37 | const Eigen::MatrixBase& product_const); 38 | 39 | template 40 | Eigen::Matrix quaternionInverseJPL( 41 | const Eigen::MatrixBase& q); 42 | 43 | 44 | namespace eigen_quaternion_helpers { 45 | 46 | template 47 | inline Eigen::Matrix Gamma( 48 | const Eigen::Matrix& phi); 49 | template 50 | inline Eigen::Quaternion ExpMap( 51 | const Eigen::Matrix& theta); 52 | 53 | inline Eigen::Quaterniond ExpMap(const Eigen::Vector3d& theta); 54 | inline Eigen::Vector3d LogMap(const Eigen::Quaterniond& q); 55 | 56 | // Rotates the quaternion p with the error theta on the tangent space: 57 | // p_plus_theta = p boxplus theta 58 | // TODO(schneith): Check if there is a better way to pass mapped quaternions. 59 | inline void Plus( 60 | const Eigen::Ref& p, 61 | const Eigen::Ref& theta, 62 | Eigen::Quaterniond* p_plus_theta); 63 | 64 | // Calculates the shortest connection respecting the manifold structure: 65 | // theta = p boxminus q 66 | // TODO(schneith): Modify to also allow Eigen::Map types. 67 | // TODO(schneith): Extend this function to also return Jacobians to reuse 68 | // computation. 69 | inline void Minus( 70 | const Eigen::Quaterniond& p, const Eigen::Quaterniond& q, 71 | Eigen::Vector3d* p_minus_q); 72 | 73 | // Calculates the Jacobian of the boxminus operator w.r.t. the two orientations 74 | // p and q to properly account for the manifold structure. 75 | // Reminder: theta = p boxminus q 76 | // TODO(schneith): Modify to also allow Eigen::Map types. 77 | // TODO(schneith): Deprecate this function and extend Minus() to also 78 | // return Jacobians to reuse computation. 79 | inline void GetBoxminusJacobians( 80 | const Eigen::Quaterniond& p, const Eigen::Quaterniond& q, 81 | Eigen::Matrix3d* J_boxminus_wrt_p, Eigen::Matrix3d* J_boxminus_wrt_q); 82 | 83 | } // namespace eigen_quaternion_helpers 84 | 85 | 86 | 87 | } // namespace common 88 | 89 | #include "./quaternion-math-inl.h" 90 | 91 | #endif // MAPLAB_COMMON_QUATERNION_MATH_H_ 92 | -------------------------------------------------------------------------------- /cmake_modules/FindEigen3.cmake: -------------------------------------------------------------------------------- 1 | # - Try to find Eigen3 lib 2 | # 3 | # This module supports requiring a minimum version, e.g. you can do 4 | # find_package(Eigen3 3.1.2) 5 | # to require version 3.1.2 or newer of Eigen3. 6 | # 7 | # Once done this will define 8 | # 9 | # EIGEN3_FOUND - system has eigen lib with correct version 10 | # EIGEN3_INCLUDE_DIR - the eigen include directory 11 | # EIGEN3_VERSION - eigen version 12 | 13 | # Copyright (c) 2006, 2007 Montel Laurent, 14 | # Copyright (c) 2008, 2009 Gael Guennebaud, 15 | # Copyright (c) 2009 Benoit Jacob 16 | # Redistribution and use is allowed according to the terms of the 2-clause BSD license. 17 | 18 | if(NOT Eigen3_FIND_VERSION) 19 | if(NOT Eigen3_FIND_VERSION_MAJOR) 20 | set(Eigen3_FIND_VERSION_MAJOR 2) 21 | endif(NOT Eigen3_FIND_VERSION_MAJOR) 22 | if(NOT Eigen3_FIND_VERSION_MINOR) 23 | set(Eigen3_FIND_VERSION_MINOR 91) 24 | endif(NOT Eigen3_FIND_VERSION_MINOR) 25 | if(NOT Eigen3_FIND_VERSION_PATCH) 26 | set(Eigen3_FIND_VERSION_PATCH 0) 27 | endif(NOT Eigen3_FIND_VERSION_PATCH) 28 | 29 | set(Eigen3_FIND_VERSION "${Eigen3_FIND_VERSION_MAJOR}.${Eigen3_FIND_VERSION_MINOR}.${Eigen3_FIND_VERSION_PATCH}") 30 | endif(NOT Eigen3_FIND_VERSION) 31 | 32 | macro(_eigen3_check_version) 33 | file(READ "${EIGEN3_INCLUDE_DIR}/Eigen/src/Core/util/Macros.h" _eigen3_version_header) 34 | 35 | string(REGEX MATCH "define[ \t]+EIGEN_WORLD_VERSION[ \t]+([0-9]+)" _eigen3_world_version_match "${_eigen3_version_header}") 36 | set(EIGEN3_WORLD_VERSION "${CMAKE_MATCH_1}") 37 | string(REGEX MATCH "define[ \t]+EIGEN_MAJOR_VERSION[ \t]+([0-9]+)" _eigen3_major_version_match "${_eigen3_version_header}") 38 | set(EIGEN3_MAJOR_VERSION "${CMAKE_MATCH_1}") 39 | string(REGEX MATCH "define[ \t]+EIGEN_MINOR_VERSION[ \t]+([0-9]+)" _eigen3_minor_version_match "${_eigen3_version_header}") 40 | set(EIGEN3_MINOR_VERSION "${CMAKE_MATCH_1}") 41 | 42 | set(EIGEN3_VERSION ${EIGEN3_WORLD_VERSION}.${EIGEN3_MAJOR_VERSION}.${EIGEN3_MINOR_VERSION}) 43 | if(${EIGEN3_VERSION} VERSION_LESS ${Eigen3_FIND_VERSION}) 44 | set(EIGEN3_VERSION_OK FALSE) 45 | else(${EIGEN3_VERSION} VERSION_LESS ${Eigen3_FIND_VERSION}) 46 | set(EIGEN3_VERSION_OK TRUE) 47 | endif(${EIGEN3_VERSION} VERSION_LESS ${Eigen3_FIND_VERSION}) 48 | 49 | if(NOT EIGEN3_VERSION_OK) 50 | 51 | message(STATUS "Eigen3 version ${EIGEN3_VERSION} found in ${EIGEN3_INCLUDE_DIR}, " 52 | "but at least version ${Eigen3_FIND_VERSION} is required") 53 | endif(NOT EIGEN3_VERSION_OK) 54 | endmacro(_eigen3_check_version) 55 | 56 | if (EIGEN3_INCLUDE_DIR) 57 | 58 | # in cache already 59 | _eigen3_check_version() 60 | set(EIGEN3_FOUND ${EIGEN3_VERSION_OK}) 61 | 62 | else (EIGEN3_INCLUDE_DIR) 63 | 64 | # specific additional paths for some OS 65 | if (WIN32) 66 | set(EIGEN_ADDITIONAL_SEARCH_PATHS ${EIGEN_ADDITIONAL_SEARCH_PATHS} "C:/Program Files/Eigen/include" "C:/Program Files (x86)/Eigen/include") 67 | endif(WIN32) 68 | 69 | find_path(EIGEN3_INCLUDE_DIR NAMES signature_of_eigen3_matrix_library 70 | PATHS 71 | ${CMAKE_INSTALL_PREFIX}/include 72 | ${EIGEN_ADDITIONAL_SEARCH_PATHS} 73 | ${KDE4_INCLUDE_DIR} 74 | PATH_SUFFIXES eigen3 eigen 75 | ) 76 | 77 | if(EIGEN3_INCLUDE_DIR) 78 | _eigen3_check_version() 79 | endif(EIGEN3_INCLUDE_DIR) 80 | 81 | include(FindPackageHandleStandardArgs) 82 | find_package_handle_standard_args(Eigen3 DEFAULT_MSG EIGEN3_INCLUDE_DIR EIGEN3_VERSION_OK) 83 | 84 | mark_as_advanced(EIGEN3_INCLUDE_DIR) 85 | 86 | endif(EIGEN3_INCLUDE_DIR) 87 | 88 | -------------------------------------------------------------------------------- /src/extern/project_error.cpp: -------------------------------------------------------------------------------- 1 | #include "extern/project_error.h" 2 | #include "PoseSpline/QuaternionSplineUtility.hpp" 3 | #include "PoseSpline/Quaternion.hpp" 4 | #include "PoseSpline/PoseLocalParameter.hpp" 5 | ProjectError::ProjectError(const Eigen::Vector3d& uv_C0): 6 | C0uv(uv_C0){ 7 | } 8 | 9 | 10 | bool ProjectError::Evaluate(double const *const *parameters, 11 | double *residuals, 12 | double **jacobians) const { 13 | return EvaluateWithMinimalJacobians(parameters, 14 | residuals, 15 | jacobians, NULL); 16 | } 17 | 18 | 19 | bool ProjectError::EvaluateWithMinimalJacobians(double const *const *parameters, 20 | double *residuals, 21 | double **jacobians, 22 | double **jacobiansMinimal) const { 23 | 24 | // T_WC 25 | Eigen::Vector3d t_WC(parameters[0][0], parameters[0][1], parameters[0][2]); 26 | Quaternion Q_WC( parameters[0][3], parameters[0][4], parameters[0][5],parameters[0][6]); 27 | 28 | // Wp 29 | Eigen::Vector3d Wp(parameters[1][0], parameters[1][1], parameters[1][2]); 30 | 31 | Eigen::Matrix3d R_WC = quatToRotMat(Q_WC); 32 | Eigen::Vector3d Cp = R_WC.transpose()*(Wp - t_WC); 33 | Eigen::Matrix error; 34 | 35 | double inv_z = 1/Cp(2); 36 | Eigen::Vector2d hat_C0uv(Cp(0)*inv_z, Cp(1)*inv_z); 37 | 38 | Eigen::Matrix H; 39 | H << 1, 0, -Cp(0)*inv_z, 40 | 0, 1, -Cp(1)*inv_z; 41 | H *= inv_z; 42 | 43 | error = hat_C0uv - C0uv.head<2>(); 44 | squareRootInformation_.setIdentity(); 45 | //squareRootInformation_ = weightScalar_* squareRootInformation_; //Weighted 46 | 47 | // weight it 48 | Eigen::Map > weighted_error(residuals); 49 | weighted_error = squareRootInformation_ * error; 50 | 51 | 52 | 53 | // calculate jacobians 54 | if(jacobians != NULL){ 55 | 56 | if(jacobians[0] != NULL){ 57 | Eigen::Matrix jacobian0_min; 58 | Eigen::Map> jacobian0(jacobians[0]); 59 | 60 | Eigen::Matrix tmp; 61 | tmp.setIdentity(); 62 | tmp.topLeftCorner(3,3) = - R_WC.transpose(); 63 | tmp.topRightCorner(3,3) = - R_WC.transpose()*crossMat((Wp - t_WC)); 64 | 65 | jacobian0_min = H*tmp; 66 | 67 | Eigen::Matrix lift; 68 | PoseLocalParameter::liftJacobian(parameters[0], lift.data()); 69 | jacobian0 = squareRootInformation_*jacobian0_min*lift; 70 | 71 | 72 | 73 | if(jacobiansMinimal != NULL && jacobiansMinimal[0] != NULL){ 74 | Eigen::Map> map_jacobian0_min(jacobiansMinimal[0]); 75 | map_jacobian0_min = squareRootInformation_*jacobian0_min; 76 | } 77 | } 78 | 79 | if(jacobians[1] != NULL){ 80 | Eigen::Matrix jacobian1_min; 81 | Eigen::Map> jacobian1(jacobians[1]); 82 | 83 | Eigen::Matrix tmp; 84 | 85 | tmp = R_WC.transpose(); 86 | jacobian1_min = H*tmp; 87 | 88 | jacobian1 = squareRootInformation_*jacobian1_min; 89 | 90 | if(jacobiansMinimal != NULL && jacobiansMinimal[1] != NULL){ 91 | Eigen::Map> map_jacobian1_min(jacobiansMinimal[1]); 92 | map_jacobian1_min = squareRootInformation_*jacobian1_min; 93 | } 94 | } 95 | 96 | } 97 | return true; 98 | } -------------------------------------------------------------------------------- /include/PoseSpline/maplab/geometry.h: -------------------------------------------------------------------------------- 1 | #ifndef MAPLAB_COMMON_GEOMETRY_H_ 2 | #define MAPLAB_COMMON_GEOMETRY_H_ 3 | #include 4 | 5 | #include 6 | // #include 7 | // #include 8 | 9 | namespace common { 10 | // typedef Aligned> VectorOfJPLQuaternia; 11 | // typedef Aligned> VectorOfPositions; 12 | 13 | template 14 | inline Eigen::Matrix skew( 15 | const Eigen::MatrixBase& vector); 16 | 17 | template 18 | void skew( 19 | const Eigen::MatrixBase& vector, 20 | Eigen::MatrixBase const& matrix_const); 21 | 22 | // // "xyz" must be a unit vector. 23 | // inline Eigen::Vector2d xyzToPhiTheta(const Eigen::Vector3d& xyz) { 24 | // return Eigen::Vector2d(atan2(xyz(1), xyz(0)), asin(xyz(2))); 25 | // } 26 | 27 | // inline Eigen::Vector3d phiThetaToXyz(const Eigen::Vector2d& pt) { 28 | // const double cos_theta = cos(pt(1)); 29 | // return Eigen::Vector3d( 30 | // cos(pt(0)) * cos_theta, sin(pt(0)) * cos_theta, sin(pt(1))); 31 | // } 32 | 33 | // // Conversion from rotation matrix to roll, pitch, yaw angles. 34 | // template 35 | // inline Eigen::Matrix 36 | // RotationMatrixToRollPitchYaw(const Eigen::MatrixBase& rot); 37 | 38 | // // Conversion from roll, pitch, yaw to rotation matrix. 39 | // template 40 | // inline Eigen::Matrix 41 | // RollPitchYawToRotationMatrix(const Eigen::MatrixBase& roll_pitch_yaw); 42 | 43 | // // Skew-symmetric (cross-product) matrix. 44 | // template 45 | // inline Eigen::Matrix SkewSymmetricMatrix( 46 | // const Eigen::MatrixBase& x); 47 | // // Compute left multiplication matrix. 48 | // template 49 | // inline Eigen::Matrix 50 | // LeftQuaternionJPLMultiplicationMatrix(const Eigen::MatrixBase& q); 51 | 52 | // // Utility method for computing the least-squares average quaternion from a 53 | // // vector of quaternia. Derivation is in Appendix A of "Mirror-Based Extrinsic 54 | // // Camera Calibration," Hesch et al. WAFR 2008. 55 | // Eigen::Matrix ComputeLSAverageQuaternionJPL( 56 | // const VectorOfJPLQuaternia& Gl_q_Gc_vector); 57 | 58 | // template