diff --git a/.idea/.gitignore b/.idea/.gitignore new file mode 100644 index 0000000..73f69e0 --- /dev/null +++ b/.idea/.gitignore @@ -0,0 +1,8 @@ +# Default ignored files +/shelf/ +/workspace.xml +# Datasource local storage ignored files +/dataSources/ +/dataSources.local.xml +# Editor-based HTTP Client requests +/httpRequests/ diff --git a/.idea/.name b/.idea/.name new file mode 100644 index 0000000..8d3ae0f --- /dev/null +++ b/.idea/.name @@ -0,0 +1 @@ +Vio_OpenCL \ No newline at end of file diff --git a/.idea/Benchmark_openCL.iml b/.idea/Benchmark_openCL.iml new file mode 100644 index 0000000..f08604b --- /dev/null +++ b/.idea/Benchmark_openCL.iml @@ -0,0 +1,2 @@ + + \ No newline at end of file diff --git a/.idea/misc.xml b/.idea/misc.xml new file mode 100644 index 0000000..79b3c94 --- /dev/null +++ b/.idea/misc.xml @@ -0,0 +1,4 @@ + + + + \ No newline at end of file diff --git a/.idea/modules.xml b/.idea/modules.xml new file mode 100644 index 0000000..2cc944c --- /dev/null +++ b/.idea/modules.xml @@ -0,0 +1,8 @@ + + + + + + + + \ No newline at end of file diff --git a/.idea/vcs.xml b/.idea/vcs.xml new file mode 100644 index 0000000..94a25f7 --- /dev/null +++ b/.idea/vcs.xml @@ -0,0 +1,6 @@ + + + + + + \ No newline at end of file diff --git a/CMakeModules/FindEigen.cmake b/CMakeModules/FindEigen.cmake new file mode 100644 index 0000000..8587367 --- /dev/null +++ b/CMakeModules/FindEigen.cmake @@ -0,0 +1,81 @@ +############################################################################### +# +# CMake script for finding the Eigen library. +# +# http://eigen.tuxfamily.org/index.php?title=Main_Page +# +# Copyright (c) 2006, 2007 Montel Laurent, +# Copyright (c) 2008, 2009 Gael Guennebaud, +# Copyright (c) 2009 Benoit Jacob +# Redistribution and use is allowed according to the terms of the 2-clause BSD +# license. +# +# +# Input variables: +# +# - Eigen_ROOT_DIR (optional): When specified, header files and libraries +# will be searched for in `${Eigen_ROOT_DIR}/include` and +# `${Eigen_ROOT_DIR}/libs` respectively, and the default CMake search order +# will be ignored. When unspecified, the default CMake search order is used. +# This variable can be specified either as a CMake or environment variable. +# If both are set, preference is given to the CMake variable. +# Use this variable for finding packages installed in a nonstandard location, +# or for enforcing that one of multiple package installations is picked up. +# +# Cache variables (not intended to be used in CMakeLists.txt files) +# +# - Eigen_INCLUDE_DIR: Absolute path to package headers. +# +# +# Output variables: +# +# - Eigen_FOUND: Boolean that indicates if the package was found +# - Eigen_INCLUDE_DIRS: Paths to the necessary header files +# - Eigen_VERSION: Version of Eigen library found +# - Eigen_DEFINITIONS: Definitions to be passed on behalf of eigen +# +# +# Example usage: +# +# # Passing the version means Eigen_FOUND will only be TRUE if a +# # version >= the provided version is found. +# find_package(Eigen 3.1.2) +# if(NOT Eigen_FOUND) +# # Error handling +# endif() +# ... +# add_definitions(${Eigen_DEFINITIONS}) +# ... +# include_directories(${Eigen_INCLUDE_DIRS} ...) +# +############################################################################### + +find_package(PkgConfig) +pkg_check_modules(PC_EIGEN eigen3) +set(EIGEN_DEFINITIONS ${PC_EIGEN_CFLAGS_OTHER}) + + +find_path(EIGEN_INCLUDE_DIR Eigen/Core + HINTS ${PC_EIGEN_INCLUDEDIR} ${PC_EIGEN_INCLUDE_DIRS} + "${Eigen_ROOT_DIR}" "$ENV{EIGEN_ROOT_DIR}" + "${EIGEN_ROOT}" "$ENV{EIGEN_ROOT}" # Backwards Compatibility + PATHS "$ENV{PROGRAMFILES}/Eigen" "$ENV{PROGRAMW6432}/Eigen" + "$ENV{PROGRAMFILES}/Eigen 3.0.0" "$ENV{PROGRAMW6432}/Eigen 3.0.0" + PATH_SUFFIXES eigen3 include/eigen3 include) + +set(EIGEN_INCLUDE_DIRS ${EIGEN_INCLUDE_DIR}) + +include(FindPackageHandleStandardArgs) +find_package_handle_standard_args(Eigen DEFAULT_MSG EIGEN_INCLUDE_DIR) + +mark_as_advanced(EIGEN_INCLUDE_DIR) + +if(EIGEN_FOUND) + message(STATUS "Eigen found (include: ${EIGEN_INCLUDE_DIRS})") +endif(EIGEN_FOUND) + + +set(Eigen_INCLUDE_DIRS ${EIGEN_INCLUDE_DIRS}) +set(Eigen_FOUND ${EIGEN_FOUND}) +set(Eigen_VERSION ${EIGEN_VERSION}) +set(Eigen_DEFINITIONS ${EIGEN_DEFINITIONS}) diff --git a/include/aligned_mem.h b/include/aligned_mem.h new file mode 100644 index 0000000..ef14742 --- /dev/null +++ b/include/aligned_mem.h @@ -0,0 +1,127 @@ +/* + * aligned_mem.h + * + * Created on: May 14, 2013 + * Author: cforster + * + * Code from libcvd: cvd/internal/aligned_mem.h + * https://github.com/edrosten/libcvd + * Licence: LGPL 2.1 + */ + +#ifndef VIKIT_ALIGNED_MEM_H_ +#define VIKIT_ALIGNED_MEM_H_ + +#include // memset +#include // memset +#include +#include + +namespace vk { +namespace aligned_mem { + + /// Check if the pointer is aligned to the specified byte granularity + inline bool + is_aligned8(const void* ptr) + { + return ((reinterpret_cast(ptr)) & 0x7) == 0; + } + + inline bool + is_aligned16(const void* ptr) + { + return ((reinterpret_cast(ptr)) & 0xF) == 0; + } + + template struct placement_delete + { + enum { Size = (1<= Size) { + placement_delete::free(buf+Size,M-Size); + placement_delete::destruct(buf); + } else { + placement_delete::free(buf, M); + } + } + }; + + template struct placement_delete + { + static inline void free(T*, size_t ) {} + }; + + inline void * aligned_alloc(size_t count, size_t alignment){ + void * mem = NULL; + assert(posix_memalign(&mem, alignment, count) == 0); + return mem; + } + + inline void aligned_free(void * memory) { + free(memory); + } + + template + inline T * aligned_alloc(size_t count, size_t alignment){ + void * data = aligned_alloc(sizeof(T)* count, alignment); + return new (data) T[count]; + } + + template + inline void aligned_free(T * memory, size_t count){ + placement_delete::free(memory, count); + aligned_free(memory); + } + + template inline void memfill(T* data, int n, const T val) + { + T* de = data + n; + for(;data < de; data++) + *data=val; + } + + template<> inline void memfill(unsigned char* data, int n, const unsigned char val) + { + memset(data, val, n); + } + + template<> inline void memfill(signed char* data, int n, const signed char val) + { + memset(data, val, n); + } + + template<> inline void memfill(char* data, int n, const char val) + { + memset(data, val, n); + } + + template + struct AlignedMem { + T* mem; + size_t count; + AlignedMem(size_t c) : count(c) { + mem = aligned_alloc(count, N); + } + ~AlignedMem() { + aligned_free(mem, count); + } + T* data() { return mem; } + const T* data() const { return mem; } + }; + +} // namespace aligned_mem +} // namespace vikit + + +#endif // VIKIT_ALIGNED_MEM_H_ diff --git a/include/math_utils.h b/include/math_utils.h new file mode 100644 index 0000000..05cd7a0 --- /dev/null +++ b/include/math_utils.h @@ -0,0 +1,172 @@ +/* + * math_utils.h + * + * Created on: Jul 20, 2012 + * Author: cforster + */ + +#ifndef MATH_UTILS_H_ +#define MATH_UTILS_H_ + + +#include +#include +#include +#include + +namespace vk +{ + +using namespace Eigen; +using namespace std; +using namespace Sophus; + +Vector3d triangulateFeatureNonLin( + const Matrix3d& R, + const Vector3d& t, + const Vector3d& feature1, + const Vector3d& feature2); + +/// Assumes the bearing vectors f_c and f_r are on the epipolar plane, i.e. +/// perfect triangulation without noise! +bool depthFromTriangulationExact( + const Matrix3d& R_r_c, + const Vector3d& t_r_c, + const Vector3d& f_r, + const Vector3d& f_c, + double& depth_in_r, + double& depth_in_c); + +double reprojError( + const Vector3d& f1, + const Vector3d& f2, + double error_multiplier2); + +double computeInliers( + const vector& features1, + const vector& features2, + const Matrix3d& R, + const Vector3d& t, + const double reproj_thresh, + double error_multiplier2, + vector& xyz_vec, + vector& inliers, + vector& outliers); + +void computeInliersOneView( + const vector & feature_sphere_vec, + const vector & xyz_vec, + const Matrix3d &R, + const Vector3d &t, + const double reproj_thresh, + const double error_multiplier2, + vector& inliers, + vector& outliers); + +//! Direct Cosine Matrix to Roll Pitch Yaw +Vector3d dcm2rpy(const Matrix3d &R); + +//! Roll Pitch Yaw to Direct Cosine Matrix +Matrix3d rpy2dcm(const Vector3d &rpy); + +//! Angle Axis parametrization to Quaternion +Quaterniond angax2quat(const Vector3d& n, const double& angle); + +//! Angle Axis parametrization to Matrix representation +Matrix3d angax2dcm(const Vector3d& n, const double& angle); + +double sampsonusError( + const Vector2d &v2Dash, + const Matrix3d& m3Essential, + const Vector2d& v2); + +inline Matrix3d sqew(const Vector3d& v) +{ + Matrix3d v_sqew; + v_sqew << 0, -v[2], v[1], + v[2], 0, -v[0], + -v[1], v[0], 0; + return v_sqew; +} + +inline double norm_max(const Eigen::VectorXd & v) +{ + double max = -1; + for (int i=0; imax){ + max = abs; + } + } + return max; +} + +inline Vector2d project2d(const Vector3d& v) +{ + return v.head<2>()/v[2]; +} + +inline Vector3d unproject2d(const Vector2d& v) +{ + return Vector3d(v[0], v[1], 1.0); +} + +inline Vector3d project3d(const Vector4d& v) +{ + return v.head<3>()/v[3]; +} + +inline Vector4d unproject3d(const Vector3d& v) +{ + return Vector4d(v[0], v[1], v[2], 1.0); +} + +template +T getMedian(vector& data_vec) +{ + assert(!data_vec.empty()); + typename vector::iterator it = data_vec.begin()+floor(data_vec.size()/2); + nth_element(data_vec.begin(), it, data_vec.end()); + return *it; +} + +inline double pyrFromZero_d(double x_0, int level) +{ + return x_0/(1< & frame_jac) +{ + const double x = xyz[0]; + const double y = xyz[1]; + const double z = xyz[2]; + const double z_2 = z*z; + + frame_jac(0,0) = -1./z *focal_length; + frame_jac(0,1) = 0; + frame_jac(0,2) = x/z_2 *focal_length; + frame_jac(0,3) = x*y/z_2 * focal_length; + frame_jac(0,4) = -(1+(x*x/z_2)) *focal_length; + frame_jac(0,5) = y/z *focal_length; + + frame_jac(1,0) = 0; + frame_jac(1,1) = -1./z *focal_length; + frame_jac(1,2) = y/z_2 *focal_length; + frame_jac(1,3) = (1+y*y/z_2) *focal_length; + frame_jac(1,4) = -x*y/z_2 *focal_length; + frame_jac(1,5) = -x/z *focal_length; +} + +} // end namespace vk + +#endif /* MATH_UTILS_H_ */ diff --git a/include/nlls_solver.h b/include/nlls_solver.h new file mode 100644 index 0000000..e3695ab --- /dev/null +++ b/include/nlls_solver.h @@ -0,0 +1,173 @@ +/* + * Abstract Nonlinear Least-Squares Solver Class + * + * nlls_solver.h + * + * Created on: Nov 5, 2012 + * Author: cforster + */ + +#ifndef LM_SOLVER_H_ +#define LM_SOLVER_H_ + +#include +#include +#include +#include +#include +#include +#include +#include + +namespace vk { + +using namespace std; +using namespace Eigen; + +/** + * \brief Abstract Class for solving nonlinear least-squares (NLLS) problems. + * + * The function implements two algorithms: Levenberg Marquardt and Gauss Newton + * + * Example implementations of this function can be found in the rpl_examples + * package: img_align_2d.cpp, img_align_3d.cpp + * + * Template Parameters: + * D : dimension of the residual + * T : type of the model, e.g. SE2, SE3 + */ + +template +class NLLSSolver { + +public: + typedef T ModelType; + enum Method{GaussNewton, LevenbergMarquardt}; + enum ScaleEstimatorType{UnitScale, TDistScale, MADScale, NormalScale}; + enum WeightFunctionType{UnitWeight, TDistWeight, TukeyWeight, HuberWeight}; + +protected: + Matrix H_; //!< Hessian approximation + Matrix Jres_; //!< Jacobian x Residual + Matrix x_; //!< update step + bool have_prior_; + ModelType prior_; + Matrix I_prior_; //!< Prior information matrix (inverse covariance) + double chi2_; + double rho_; + Method method_; + + /// If the flag linearize_system is set, the function must also compute the + /// Jacobian and set the member variables H_, Jres_ + virtual double + computeResiduals (const ModelType& model, + bool linearize_system, + bool compute_weight_scale) = 0; + virtual double + computeResiduals (bool linearize_system, + bool compute_weight_scale) = 0; + + /// Solve the linear system H*x = Jres. This function must set the update + /// step in the member variable x_. Must return true if the system could be + /// solved and false if it was singular. + virtual int + solve () = 0; + + virtual void + update (const ModelType& old_model, ModelType& new_model) = 0; + virtual void + update () = 0; + + virtual void + applyPrior (const ModelType& current_model) { } + + virtual void + startIteration () { } + + virtual void + finishIteration () { } + + virtual void + finishTrial () { } + +public: + + /// Damping parameter. If mu > 0, coefficient matrix is positive definite, this + /// ensures that x is a descent direction. If mu is large, x is a short step in + /// the steepest direction. This is good if the current iterate is far from the + /// solution. If mu is small, LM approximates gauss newton iteration and we + /// have (almost) quadratic convergence in the final stages. + double mu_init_, mu_; + double nu_init_, nu_; //!< Increase factor of mu after fail + size_t n_iter_init_, n_iter_; //!< Number of Iterations + size_t n_trials_; //!< Number of trials + size_t n_trials_max_; //!< Max number of trials + size_t n_meas_; //!< Number of measurements + bool stop_; //!< Stop flag + bool verbose_; //!< Output Statistics + double eps_; //!< Stop if update norm is smaller than eps + size_t iter_; //!< Current Iteration + + // robust least squares + bool use_weights_; + float scale_=2.0; + vk::robust_cost::ScaleEstimatorPtr scale_estimator_; + vk::robust_cost::WeightFunctionPtr weight_function_; + + NLLSSolver() : + have_prior_(false), + method_(LevenbergMarquardt), + mu_init_(0.01f), + mu_(mu_init_), + nu_init_(2.0), + nu_(nu_init_), + n_iter_init_(15), + n_iter_(n_iter_init_), + n_trials_(0), + n_trials_max_(5), + n_meas_(0), + stop_(false), + verbose_(true), + eps_(0.0000000001), + iter_(0), + use_weights_(false), + scale_estimator_(NULL), + weight_function_(NULL) + { } + + virtual ~NLLSSolver() {} + + /// Calls the GaussNewton or LevenbergMarquardt optimization strategy + void optimize(ModelType& model); + + /// Gauss Newton optimization strategy + void optimizeGaussNewton(); + + /// Levenberg Marquardt optimization strategy + void optimizeLevenbergMarquardt(ModelType& model); + + /// Specify the robust cost that should be used and the appropriate scale estimator + void setRobustCostFunction( + ScaleEstimatorType scale_estimator, + WeightFunctionType weight_function); + + /// Add prior to optimization. + void setPrior( + const ModelType& prior, + const Matrix& Information); + + /// Reset all parameters to restart the optimization + void reset(); + + /// Get the squared error + const double& getChi2() const; + + /// The Information matrix is equal to the inverse covariance matrix. + const Matrix& getInformationMatrix() const; +}; + +} // end namespace vk + +#include "nlls_solver_impl.hpp" + +#endif /* LM_SOLVER_H_ */ diff --git a/include/nlls_solver_impl.hpp b/include/nlls_solver_impl.hpp new file mode 100644 index 0000000..edc3174 --- /dev/null +++ b/include/nlls_solver_impl.hpp @@ -0,0 +1,315 @@ +/* + * Abstract Nonlinear Least-Squares Solver Class + * + * nlls_solver.h + * + * Created on: Nov 5, 2012 + * Author: cforster + */ + +#ifndef LM_SOLVER_IMPL_HPP_ +#define LM_SOLVER_IMPL_HPP_ + +#include + +template +void vk::NLLSSolver::optimize(ModelType& model) +{ + if(method_ == GaussNewton) + optimizeGaussNewton(); + else if(method_ == LevenbergMarquardt) + optimizeLevenbergMarquardt(model); +} + +template +void vk::NLLSSolver::optimizeGaussNewton() +{ + // Compute weight scale + if(use_weights_) + computeResiduals(false, true); + // perform iterative estimation + for (iter_ = 0; iter_ 0 && new_chi2 > chi2_) || stop_) + { + if(verbose_) + { + std::cerr << "It. " << iter_ + << "\t Failure" + << "\t new_chi2 = " << new_chi2 + << "\t Error increased. Stop optimizing.\n"; + } + break; + } + // update the model + update(); + chi2_ = new_chi2; + if(verbose_) + { + std::cerr << "It. " << iter_ + << "\t Success" + << "\t new_chi2 = " << new_chi2 + << "\t x_norm = " << vk::norm_max(x_)<<'\n'; + } + // stop when converged, i.e. update step too small + if(vk::norm_max(x_)<=eps_) + break; + } +} + +template +void vk::NLLSSolver::optimizeLevenbergMarquardt(ModelType& model) +{ + // Compute weight scale + if(use_weights_) + computeResiduals(model, false, true); + + // compute the initial error + chi2_ = computeResiduals(model, true, false); + + if(verbose_) + cout << "init chi2 = " << chi2_ + << "\t n_meas = " << n_meas_ + << endl; + + // TODO: compute initial lambda + // Hartley and Zisserman: "A typical init value of lambda is 10^-3 times the + // average of the diagonal elements of J'J" + + // Compute Initial Lambda + if(mu_ < 0) + { + double H_max_diag = 0; + double tau = 1e-4; + for(size_t j=0; j::Identity(D,D); + Jres_.setZero(); + + // compute initial error + n_meas_ = 0; + computeResiduals(model, true, false); + + // add damping term: + H_ += (H_.diagonal()*mu_).asDiagonal(); + + // add prior + if(have_prior_) + applyPrior(model); + + // solve the linear system + if(solve()) + { + // update the model + update(model, new_model); + + // compute error with new model and compare to old error + n_meas_ = 0; + new_chi2 = computeResiduals(new_model, true, false); + rho_ = chi2_-new_chi2; + } + else + { + // matrix was singular and could not be computed + cout << "Matrix is close to singular!" << endl; + cout << "H = " << H_ << endl; + cout << "Jres = " << Jres_ << endl; + rho_ = -1; + } + + if(rho_>0) + { + // update decrased the error -> success + model = new_model; + chi2_ = new_chi2; + stop_ = vk::norm_max(x_)<=eps_ ? true : false; + mu_ *= max(1./3., min(1.-pow(2*rho_-1,3), 2./3.)); + nu_ = 2.; + if(verbose_) + { + cout << "It. " << iter_ + << "\t Trial " << n_trials_ + << "\t Success" + << "\t n_meas = " << n_meas_ + << "\t new_chi2 = " << new_chi2 + << "\t mu = " << mu_ + << "\t nu = " << nu_ + << "\t x_norm = " << vk::norm_max(x_) + << endl; + } + } + else + { + if(sign){ + if(verbose_) + { + cout << "It. " << iter_ + << "\t Trial " << n_trials_ + << "\t Failure" + << "\t n_meas = " << n_meas_ + << "\t new_chi2 = " << new_chi2 + << "\t mu = " << mu_ + << "\t nu = " << nu_ + << "\t x_norm = " << vk::norm_max(x_) + << endl; + } + mu_ /= nu_; + nu_ /= 2.; + ++n_trials_; + if (n_trials_ >= n_trials_max_) + stop_ = true; + sign=false; + }else{ + if(verbose_) + { + cout << "It. " << iter_ + << "\t Trial " << n_trials_ + << "\t Failure" + << "\t n_meas = " << n_meas_ + << "\t new_chi2 = " << new_chi2 + << "\t mu = " << mu_ + << "\t nu = " << nu_ + << "\t x_norm = " << vk::norm_max(x_) + << endl; + } + mu_ *= nu_; + nu_ *= 2.; + ++n_trials_; + if (n_trials_ >= n_trials_max_) + stop_ = true; + sign=true; + } + + } + + finishTrial(); + + } while(!(rho_>0 || stop_)); + if (stop_) + break; + + finishIteration(); + } +} + + +template +void vk::NLLSSolver::setRobustCostFunction( + ScaleEstimatorType scale_estimator, + WeightFunctionType weight_function) +{ + switch(scale_estimator) + { + case TDistScale: + if(verbose_) + printf("Using TDistribution Scale Estimator\n"); + scale_estimator_.reset(new robust_cost::TDistributionScaleEstimator()); + use_weights_=true; + break; + case MADScale: + if(verbose_) + printf("Using MAD Scale Estimator\n"); + scale_estimator_.reset(new robust_cost::MADScaleEstimator()); + use_weights_=true; + break; + case NormalScale: + if(verbose_) + printf("Using Normal Scale Estimator\n"); + scale_estimator_.reset(new robust_cost::NormalDistributionScaleEstimator()); + use_weights_=true; + break; + default: + if(verbose_) + printf("Using Unit Scale Estimator\n"); + scale_estimator_.reset(new robust_cost::UnitScaleEstimator()); + use_weights_=false; + } + + switch(weight_function) + { + case TDistWeight: + if(verbose_) + printf("Using TDistribution Weight Function\n"); + weight_function_.reset(new robust_cost::TDistributionWeightFunction()); + break; + case TukeyWeight: + if(verbose_) + printf("Using Tukey Weight Function\n"); + weight_function_.reset(new robust_cost::TukeyWeightFunction()); + break; + case HuberWeight: + if(verbose_) + printf("Using Huber Weight Function\n"); + weight_function_.reset(new robust_cost::HuberWeightFunction()); + break; + default: + if(verbose_) + printf("Using Unit Weight Function\n"); + weight_function_.reset(new robust_cost::UnitWeightFunction()); + } +} + +template +void vk::NLLSSolver::setPrior( + const T& prior, + const Matrix& Information) +{ + have_prior_ = true; + prior_ = prior; + I_prior_ = Information; +} + +template +void vk::NLLSSolver::reset() +{ + have_prior_ = false; + chi2_ = 1e10; + mu_ = mu_init_; + nu_ = nu_init_; + n_meas_ = 0; + n_iter_ = n_iter_init_; + iter_ = 0; + stop_ = false; +} + +template +inline const double& vk::NLLSSolver::getChi2() const +{ + return chi2_; +} + +template +inline const vk::Matrix& vk::NLLSSolver::getInformationMatrix() const +{ + return H_; +} + +#endif /* LM_SOLVER_IMPL_HPP_ */ diff --git a/include/robust_cost.h b/include/robust_cost.h new file mode 100644 index 0000000..d03881a --- /dev/null +++ b/include/robust_cost.h @@ -0,0 +1,156 @@ +/** +* This file is part of dvo. +* +* Copyright 2012 Christian Kerl (Technical University of Munich) +* For more information see . +* +* dvo is free software: you can redistribute it and/or modify +* it under the terms of the GNU General Public License as published by +* the Free Software Foundation, either version 3 of the License, or +* (at your option) any later version. +* +* dvo is distributed in the hope that it will be useful, +* but WITHOUT ANY WARRANTY; without even the implied warranty of +* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the +* GNU General Public License for more details. +* +* You should have received a copy of the GNU General Public License +* along with dvo. If not, see . +*/ + +#ifndef VIKIT_ROBUST_COST_H_ +#define VIKIT_ROBUST_COST_H_ + +#include +#include +#include +namespace vk { +namespace robust_cost { + +// interface for scale estimators +class ScaleEstimator +{ +public: + virtual ~ScaleEstimator() {}; + virtual float compute(std::vector& errors) const = 0; +}; +typedef std::shared_ptr ScaleEstimatorPtr; + +class UnitScaleEstimator : public ScaleEstimator +{ +public: + UnitScaleEstimator() {} + virtual ~UnitScaleEstimator() {} + virtual float compute(std::vector& errors) const { return 1.0f; }; +}; + +// estimates scale by fitting a t-distribution to the data with the given degrees of freedom +class TDistributionScaleEstimator : public ScaleEstimator +{ +public: + TDistributionScaleEstimator(const float dof = DEFAULT_DOF); + virtual ~TDistributionScaleEstimator() {}; + virtual float compute(std::vector& errors) const; + + static const float DEFAULT_DOF; + static const float INITIAL_SIGMA; +protected: + float dof_; + float initial_sigma_; +}; + +// estimates scale by computing the median absolute deviation +class MADScaleEstimator : public ScaleEstimator +{ +public: + MADScaleEstimator() {}; + virtual ~MADScaleEstimator() {}; + virtual float compute(std::vector& errors) const; + +private: + static const float NORMALIZER;; +}; + +// estimates scale by computing the standard deviation +class NormalDistributionScaleEstimator : public ScaleEstimator +{ +public: + NormalDistributionScaleEstimator() {}; + virtual ~NormalDistributionScaleEstimator() {}; + virtual float compute(std::vector& errors) const; +private: +}; + +/** + * Interface for weight functions. A weight function is the first derivative of a symmetric robust function p(sqrt(t)). + * The errors are assumed to be normalized to unit variance. + * + * See: + * "Lucas-Kanade 20 Years On: A Unifying Framework: Part 2" - Page 23, Equation (54) + */ +class WeightFunction +{ +public: + virtual ~WeightFunction() {}; + virtual float value(const float& x) const = 0; + virtual void configure(const float& param) {}; +}; +typedef std::shared_ptr WeightFunctionPtr; + +class UnitWeightFunction : public WeightFunction +{ +public: + UnitWeightFunction() {}; + virtual ~UnitWeightFunction() {}; + virtual float value(const float& x) const { return 1.0f; }; +}; + +/** + * Tukey's hard re-descending function. + * + * See: + * http://en.wikipedia.org/wiki/Redescending_M-estimator + */ +class TukeyWeightFunction : public WeightFunction +{ +public: + TukeyWeightFunction(const float b = DEFAULT_B); + virtual ~TukeyWeightFunction() {}; + virtual float value(const float& x) const; + virtual void configure(const float& param); + + static const float DEFAULT_B; +private: + float b_square; +}; + +class TDistributionWeightFunction : public WeightFunction +{ +public: + TDistributionWeightFunction(const float dof = DEFAULT_DOF); + virtual ~TDistributionWeightFunction() {}; + virtual float value(const float& x) const; + virtual void configure(const float& param); + + static const float DEFAULT_DOF; +private: + float dof_; + float normalizer_; +}; + +class HuberWeightFunction : public WeightFunction +{ +public: + HuberWeightFunction(const float k = DEFAULT_K); + virtual ~HuberWeightFunction() {}; + virtual float value(const float& x) const; + virtual void configure(const float& param); + + static const float DEFAULT_K; +private: + float k; +}; + +} // namespace robust_cost +} // namespace vk +#endif // VIKIT_ROBUST_COST_H_ diff --git a/include/sparse_img_align_gpu.h b/include/sparse_img_align_gpu.h new file mode 100644 index 0000000..6e691a0 --- /dev/null +++ b/include/sparse_img_align_gpu.h @@ -0,0 +1,75 @@ +// This file is part of SVO - Semi-direct Visual Odometry. +// +// Copyright (C) 2014 Christian Forster +// (Robotics and Perception Group, University of Zurich, Switzerland). +// +// SVO is free software: you can redistribute it and/or modify it under the +// terms of the GNU General Public License as published by the Free Software +// Foundation, either version 3 of the License, or any later version. +// +// SVO is distributed in the hope that it will be useful, but WITHOUT ANY +// WARRANTY; without even the implied warranty of MERCHANTABILITY or FITNESS +// FOR A PARTICULAR PURPOSE. See the GNU General Public License for more details. +// +// You should have received a copy of the GNU General Public License +// along with this program. If not, see . + +#ifndef VIO_SPARSE_IMG_ALIGN_GPU_H_ +#define VIO_SPARSE_IMG_ALIGN_GPU_H_ + +#include +#include +#include +#include +#include + +namespace vio { + +class Feature; + +/// Optimize the pose of the frame by minimizing the photometric error of feature patches. +class SparseImgAlignGpu : public vk::NLLSSolver<3, Sophus::SE2> +{ + static const int patch_halfsize_ = 12; + static const int patch_size_ = 12*patch_halfsize_; + static const int patch_area_ = patch_size_*patch_size_; +public: + + SparseImgAlignGpu( + int max_level, + int min_level, + int n_iter, + Method method, + bool verbose, + opencl* residual); + + size_t run(); + void data_reader(std::string filename, float *rp, float *cp, float *c, float *fts, float* fts_px); + int feature_counter(std::string filename); + // FILE* data= nullptr; + +protected: + int level_; //!< current pyramid level on which the optimization runs. + int max_level_; //!< coarsest pyramid level for the alignment. + int min_level_; //!< finest pyramid level for the alignment. + size_t feature_counter_=0; + opencl* residual_= nullptr; + + // cache: + std::vector visible_fts_; + virtual double computeResiduals(const ModelType& model, + bool linearize_system, + bool compute_weight_scale) { + return 0.0; + }; + virtual double computeResiduals(bool linearize_system, bool compute_weight_scale = false); + virtual int solve(); + virtual void update(); + virtual void update(const ModelType& old_model, ModelType& new_model) { }; + virtual void startIteration(); + virtual void finishIteration(); +}; + +} // namespace vio + +#endif // VIO_SPARSE_IMG_ALIGN_GPU_H_ diff --git a/include/vision.h b/include/vision.h new file mode 100644 index 0000000..7a9c759 --- /dev/null +++ b/include/vision.h @@ -0,0 +1,80 @@ +/* + * vision.h + * + * Created on: May 14, 2013 + * Author: cforster + */ + +#ifndef VIKIT_VISION_H_ +#define VIKIT_VISION_H_ + +#include +#include +#include + +namespace vk +{ + +//! Return value between 0 and 1 +//! WARNING This function does not check whether the x/y is within the border +inline float +interpolateMat_32f(const cv::Mat& mat, float u, float v) +{ + assert(mat.type()==CV_32F); + float x = floor(u); + float y = floor(v); + float subpix_x = u-x; + float subpix_y = v-y; + float wx0 = 1.0-subpix_x; + float wx1 = subpix_x; + float wy0 = 1.0-subpix_y; + float wy1 = subpix_y; + + float val00 = mat.at(y,x); + float val10 = mat.at(y,x+1); + float val01 = mat.at(y+1,x); + float val11 = mat.at(y+1,x+1); + return (wx0*wy0)*val00 + (wx1*wy0)*val10 + (wx0*wy1)*val01 + (wx1*wy1)*val11; +} + +//! Return value between 0 and 255 +//! WARNING This function does not check whether the x/y is within the border +inline float +interpolateMat_8u(const cv::Mat& mat, float u, float v) +{ + if(mat.empty())return 0.0; + if(mat.type()!=CV_8U)return 0.0; + int x = floor(u); + int y = floor(v); + float subpix_x = u-x; + float subpix_y = v-y; + + float w00 = (1.0f-subpix_x)*(1.0f-subpix_y); + float w01 = (1.0f-subpix_x)*subpix_y; + float w10 = subpix_x*(1.0f-subpix_y); + float w11 = 1.0f - w00 - w01 - w10; + + const int stride = mat.step.p[0]; + unsigned char* ptr = mat.data + y*stride + x; + if(isnan(w00) || isnan(w01 )||isnan( w10) || isnan(w11) || isinf(w00) || isinf(w01)|| isinf(w10)|| isinf(w11))return 0.0; + if(ptr[0] == NULL || ptr[1]== NULL || ptr[stride]==NULL ||ptr[stride+1]==NULL)return 0.0; + return w00*ptr[0] + w01*ptr[stride] + w10*ptr[1] + w11*ptr[stride+1]; +} + +void halfSample(const cv::Mat& in, cv::Mat& out); + +float shiTomasiScore(const cv::Mat& img, int u, int v); + +void calcSharrDeriv(const cv::Mat& src, cv::Mat& dst); + +#ifdef __SSE2__ + +/// Used to convert a Kinect depthmap +/// Code by Christian Kerl DVO, GPL Licence +void convertRawDepthImageSse_16u_to_32f(cv::Mat& depth_16u, cv::Mat& depth_32f, float scale); + +#endif + +} // namespace vk + +#endif // VIKIT_VISION_H_ diff --git a/kernel/Freak c++ files/freak.cpp b/kernel/Freak c++ files/freak.cpp new file mode 100644 index 0000000..8d1b61d --- /dev/null +++ b/kernel/Freak c++ files/freak.cpp @@ -0,0 +1,844 @@ +// freak.cpp +// +// Copyright (C) 2011-2012 Signal processing laboratory 2, EPFL, +// Kirell Benzi (kirell.benzi@epfl.ch), +// Raphael Ortiz (raphael.ortiz@a3.epfl.ch) +// Alexandre Alahi (alexandre.alahi@epfl.ch) +// and Pierre Vandergheynst (pierre.vandergheynst@epfl.ch) +// +// Redistribution and use in source and binary forms, with or without modification, +// are permitted provided that the following conditions are met: +// +// * Redistribution's of source code must retain the above copyright notice, +// this list of conditions and the following disclaimer. +// +// * Redistribution's in binary form must reproduce the above copyright notice, +// this list of conditions and the following disclaimer in the documentation +// and/or other materials provided with the distribution. +// +// * The name of the copyright holders may not be used to endorse or promote products +// derived from this software without specific prior written permission. +// +// This software is provided by the copyright holders and contributors "as is" and +// any express or implied warranties, including, but not limited to, the implied +// warranties of merchantability and fitness for a particular purpose are disclaimed. +// In no event shall the Intel Corporation or contributors be liable for any direct, +// indirect, incidental, special, exemplary, or consequential damages +// (including, but not limited to, procurement of substitute goods or services; +// loss of use, data, or profits; or business interruption) however caused +// and on any theory of liability, whether in contract, strict liability, +// or tort (including negligence or otherwise) arising in any way out of +// the use of this software, even if advised of the possibility of such damage. + +#include "precomp.hpp" +#include +#include +#include +#include +#include +#include +#include +#include +#include + +namespace cv +{ +namespace xfeatures2d +{ + +/*! + FREAK implementation + */ +class FREAK_Impl CV_FINAL : public FREAK +{ +public: + /** Constructor + * @param orientationNormalized enable orientation normalization + * @param scaleNormalized enable scale normalization + * @param patternScale scaling of the description pattern + * @param nbOctave number of octaves covered by the detected keypoints + * @param selectedPairs (optional) user defined selected pairs + */ + explicit FREAK_Impl( bool orientationNormalized = true, + bool scaleNormalized = true, + float patternScale = 22.0f, + int nOctaves = 4, + const std::vector& selectedPairs = std::vector()); + + virtual ~FREAK_Impl(); + + /** returns the descriptor length in bytes */ + virtual int descriptorSize() const CV_OVERRIDE; + + /** returns the descriptor type */ + virtual int descriptorType() const CV_OVERRIDE; + + /** returns the default norm type */ + virtual int defaultNorm() const CV_OVERRIDE; + + /** select the 512 "best description pairs" + * @param images grayscale images set + * @param keypoints set of detected keypoints + * @param corrThresh correlation threshold + * @param verbose print construction information + * @return list of best pair indexes + */ + std::vector selectPairs( const std::vector& images, std::vector >& keypoints, + const double corrThresh = 0.7, bool verbose = true ); + virtual void compute( InputArray image, std::vector& keypoints, OutputArray descriptors ) CV_OVERRIDE; + +protected: + + void buildPattern(); + + template + imgType meanIntensity( InputArray image, InputArray integral, const float kp_x, const float kp_y, + const unsigned int scale, const unsigned int rot, const unsigned int point ); + + template + void computeDescriptors( InputArray image, std::vector& keypoints, OutputArray descriptors ); + + template + void extractDescriptor(srcMatType *pointsValue, void ** ptr); + + bool orientationNormalized; //true if the orientation is normalized, false otherwise + bool scaleNormalized; //true if the scale is normalized, false otherwise + double patternScale; //scaling of the pattern + int nOctaves; //number of octaves + bool extAll; // true if all pairs need to be extracted for pairs selection + + double patternScale0; + int nOctaves0; + std::vector selectedPairs0; + + struct PatternPoint + { + float x; // x coordinate relative to center + float y; // x coordinate relative to center + float sigma; // Gaussian smoothing sigma + }; + + struct DescriptionPair + { + uchar i; // index of the first point + uchar j; // index of the second point + }; + + struct OrientationPair + { + uchar i; // index of the first point + uchar j; // index of the second point + int weight_dx; // dx/(norm_sq))*4096 + int weight_dy; // dy/(norm_sq))*4096 + }; + + std::vector patternLookup; // look-up table for the pattern points (position+sigma of all points at all scales and orientation) + int patternSizes[NB_SCALES]; // size of the pattern at a specific scale (used to check if a point is within image boundaries) + DescriptionPair descriptionPairs[NB_PAIRS]; + OrientationPair orientationPairs[NB_ORIENPAIRS]; +}; + +static const double FREAK_LOG2 = 0.693147180559945; +static const int FREAK_NB_ORIENTATION = 256; +static const int FREAK_NB_POINTS = 43; +static const int FREAK_SMALLEST_KP_SIZE = 7; // smallest size of keypoints + +// default pairs +static const int FREAK_DEF_PAIRS[FREAK_Impl::NB_PAIRS] = +{ + 404,431,818,511,181,52,311,874,774,543,719,230,417,205,11, + 560,149,265,39,306,165,857,250,8,61,15,55,717,44,412, + 592,134,761,695,660,782,625,487,549,516,271,665,762,392,178, + 796,773,31,672,845,548,794,677,654,241,831,225,238,849,83, + 691,484,826,707,122,517,583,731,328,339,571,475,394,472,580, + 381,137,93,380,327,619,729,808,218,213,459,141,806,341,95, + 382,568,124,750,193,749,706,843,79,199,317,329,768,198,100, + 466,613,78,562,783,689,136,838,94,142,164,679,219,419,366, + 418,423,77,89,523,259,683,312,555,20,470,684,123,458,453,833, + 72,113,253,108,313,25,153,648,411,607,618,128,305,232,301,84, + 56,264,371,46,407,360,38,99,176,710,114,578,66,372,653, + 129,359,424,159,821,10,323,393,5,340,891,9,790,47,0,175,346, + 236,26,172,147,574,561,32,294,429,724,755,398,787,288,299, + 769,565,767,722,757,224,465,723,498,467,235,127,802,446,233, + 544,482,800,318,16,532,801,441,554,173,60,530,713,469,30, + 212,630,899,170,266,799,88,49,512,399,23,500,107,524,90, + 194,143,135,192,206,345,148,71,119,101,563,870,158,254,214, + 276,464,332,725,188,385,24,476,40,231,620,171,258,67,109, + 844,244,187,388,701,690,50,7,850,479,48,522,22,154,12,659, + 736,655,577,737,830,811,174,21,237,335,353,234,53,270,62, + 182,45,177,245,812,673,355,556,612,166,204,54,248,365,226, + 242,452,700,685,573,14,842,481,468,781,564,416,179,405,35, + 819,608,624,367,98,643,448,2,460,676,440,240,130,146,184, + 185,430,65,807,377,82,121,708,239,310,138,596,730,575,477, + 851,797,247,27,85,586,307,779,326,494,856,324,827,96,748, + 13,397,125,688,702,92,293,716,277,140,112,4,80,855,839,1, + 413,347,584,493,289,696,19,751,379,76,73,115,6,590,183,734, + 197,483,217,344,330,400,186,243,587,220,780,200,793,246,824, + 41,735,579,81,703,322,760,720,139,480,490,91,814,813,163, + 152,488,763,263,425,410,576,120,319,668,150,160,302,491,515, + 260,145,428,97,251,395,272,252,18,106,358,854,485,144,550, + 131,133,378,68,102,104,58,361,275,209,697,582,338,742,589, + 325,408,229,28,304,191,189,110,126,486,211,547,533,70,215, + 670,249,36,581,389,605,331,518,442,822 +}; + +// used to sort pairs during pairs selection +struct PairStat +{ + double mean; + int idx; +}; + +struct sortMean +{ + bool operator()( const PairStat& a, const PairStat& b ) const + { + return a.mean < b.mean; + } +}; + +void FREAK_Impl::buildPattern() +{ + if( patternScale == patternScale0 && nOctaves == nOctaves0 && !patternLookup.empty() ) + return; + + nOctaves0 = nOctaves; + patternScale0 = patternScale; + +patternLookup.resize(FREAK::NB_SCALES*FREAK_NB_ORIENTATION*FREAK_NB_POINTS); + double scaleStep = std::pow(2.0, (double)(nOctaves)/FREAK::NB_SCALES ); // 2 ^ ( (nOctaves-1) /nbScales) + double scalingFactor, alpha, beta, theta = 0; + + // pattern definition, radius normalized to 1.0 (outer point position+sigma=1.0) + const int n[8] = {6,6,6,6,6,6,6,1}; // number of points on each concentric circle (from outer to inner) + const double bigR(2.0/3.0); // bigger radius + const double smallR(2.0/24.0); // smaller radius + const double unitSpace( (bigR-smallR)/21.0 ); // define spaces between concentric circles (from center to outer: 1,2,3,4,5,6) + // radii of the concentric cirles (from outer to inner) + const double radius[8] = {bigR, bigR-6*unitSpace, bigR-11*unitSpace, bigR-15*unitSpace, bigR-18*unitSpace, bigR-20*unitSpace, smallR, 0.0}; + // sigma of pattern points (each group of 6 points on a concentric cirle has the same sigma) + const double sigma[8] = {radius[0]/2.0, radius[1]/2.0, radius[2]/2.0, + radius[3]/2.0, radius[4]/2.0, radius[5]/2.0, + radius[6]/2.0, radius[6]/2.0 + }; + // fill the lookup table + for( int scaleIdx=0; scaleIdx < FREAK::NB_SCALES; ++scaleIdx ) + { + patternSizes[scaleIdx] = 0; // proper initialization + scalingFactor = std::pow(scaleStep,scaleIdx); //scale of the pattern, scaleStep ^ scaleIdx + + for( int orientationIdx = 0; orientationIdx < FREAK_NB_ORIENTATION; ++orientationIdx ) + { + theta = double(orientationIdx)* 2*CV_PI/double(FREAK_NB_ORIENTATION); // orientation of the pattern + int pointIdx = 0; + + PatternPoint* patternLookupPtr = &patternLookup[0]; + for( size_t i = 0; i < 8; ++i ) + { + for( int k = 0 ; k < n[i]; ++k ) + { + beta = CV_PI/n[i] * (i%2); // orientation offset so that groups of points on each circles are staggered + alpha = double(k)* 2*CV_PI/double(n[i])+beta+theta; + + // add the point to the look-up table + PatternPoint& point = patternLookupPtr[ scaleIdx*FREAK_NB_ORIENTATION*FREAK_NB_POINTS+orientationIdx*FREAK_NB_POINTS+pointIdx ]; + point.x = static_cast(radius[i] * cos(alpha) * scalingFactor * patternScale); + point.y = static_cast(radius[i] * sin(alpha) * scalingFactor * patternScale); + point.sigma = static_cast(sigma[i] * scalingFactor * patternScale); + + // adapt the sizeList if necessary + const int sizeMax = static_cast(ceil((radius[i]+sigma[i])*scalingFactor*patternScale)) + 1; + if( patternSizes[scaleIdx] < sizeMax ) + patternSizes[scaleIdx] = sizeMax; + + ++pointIdx; + } + } + } + } + + // build the list of orientation pairs + orientationPairs[0].i=0; orientationPairs[0].j=3; orientationPairs[1].i=1; orientationPairs[1].j=4; orientationPairs[2].i=2; orientationPairs[2].j=5; + orientationPairs[3].i=0; orientationPairs[3].j=2; orientationPairs[4].i=1; orientationPairs[4].j=3; orientationPairs[5].i=2; orientationPairs[5].j=4; + orientationPairs[6].i=3; orientationPairs[6].j=5; orientationPairs[7].i=4; orientationPairs[7].j=0; orientationPairs[8].i=5; orientationPairs[8].j=1; + + orientationPairs[9].i=6; orientationPairs[9].j=9; orientationPairs[10].i=7; orientationPairs[10].j=10; orientationPairs[11].i=8; orientationPairs[11].j=11; + orientationPairs[12].i=6; orientationPairs[12].j=8; orientationPairs[13].i=7; orientationPairs[13].j=9; orientationPairs[14].i=8; orientationPairs[14].j=10; + orientationPairs[15].i=9; orientationPairs[15].j=11; orientationPairs[16].i=10; orientationPairs[16].j=6; orientationPairs[17].i=11; orientationPairs[17].j=7; + + orientationPairs[18].i=12; orientationPairs[18].j=15; orientationPairs[19].i=13; orientationPairs[19].j=16; orientationPairs[20].i=14; orientationPairs[20].j=17; + orientationPairs[21].i=12; orientationPairs[21].j=14; orientationPairs[22].i=13; orientationPairs[22].j=15; orientationPairs[23].i=14; orientationPairs[23].j=16; + orientationPairs[24].i=15; orientationPairs[24].j=17; orientationPairs[25].i=16; orientationPairs[25].j=12; orientationPairs[26].i=17; orientationPairs[26].j=13; + + orientationPairs[27].i=18; orientationPairs[27].j=21; orientationPairs[28].i=19; orientationPairs[28].j=22; orientationPairs[29].i=20; orientationPairs[29].j=23; + orientationPairs[30].i=18; orientationPairs[30].j=20; orientationPairs[31].i=19; orientationPairs[31].j=21; orientationPairs[32].i=20; orientationPairs[32].j=22; + orientationPairs[33].i=21; orientationPairs[33].j=23; orientationPairs[34].i=22; orientationPairs[34].j=18; orientationPairs[35].i=23; orientationPairs[35].j=19; + + orientationPairs[36].i=24; orientationPairs[36].j=27; orientationPairs[37].i=25; orientationPairs[37].j=28; orientationPairs[38].i=26; orientationPairs[38].j=29; + orientationPairs[39].i=30; orientationPairs[39].j=33; orientationPairs[40].i=31; orientationPairs[40].j=34; orientationPairs[41].i=32; orientationPairs[41].j=35; + orientationPairs[42].i=36; orientationPairs[42].j=39; orientationPairs[43].i=37; orientationPairs[43].j=40; orientationPairs[44].i=38; orientationPairs[44].j=41; + + for( unsigned m = FREAK::NB_ORIENPAIRS; m--; ) + { + const float dx = patternLookup[orientationPairs[m].i].x-patternLookup[orientationPairs[m].j].x; + const float dy = patternLookup[orientationPairs[m].i].y-patternLookup[orientationPairs[m].j].y; + const float norm_sq = (dx*dx+dy*dy); + orientationPairs[m].weight_dx = cvRound((dx/(norm_sq))*4096.0); + orientationPairs[m].weight_dy = cvRound((dy/(norm_sq))*4096.0); + } + + // build the list of description pairs + std::vector allPairs; + for( unsigned int i = 1; i < (unsigned int)FREAK_NB_POINTS; ++i ) + { + // (generate all the pairs) + for( unsigned int j = 0; (unsigned int)j < i; ++j ) + { + DescriptionPair pair = {(uchar)i,(uchar)j}; + allPairs.push_back(pair); + } + } + // Input vector provided + if( !selectedPairs0.empty() ) + { + if( (int)selectedPairs0.size() == FREAK::NB_PAIRS ) + { + for( int i = 0; i < FREAK::NB_PAIRS; ++i ) + descriptionPairs[i] = allPairs[selectedPairs0.at(i)]; + } + else + { + CV_Error(Error::StsVecLengthErr, "Input vector does not match the required size"); + } + } + else // default selected pairs + { + for( int i = 0; i < FREAK::NB_PAIRS; ++i ) + descriptionPairs[i] = allPairs[FREAK_DEF_PAIRS[i]]; + } +} + +void FREAK_Impl::compute( InputArray _image, std::vector& keypoints, OutputArray _descriptors ) +{ + Mat image = _image.getMat(); + if( image.empty() ) + return; + if( keypoints.empty() ) + return; + + ((FREAK_Impl*)this)->buildPattern(); + + // Convert to gray if not already + Mat grayImage; + if( image.channels() == 3 || image.channels() == 4 ) + cvtColor(image, grayImage, COLOR_BGR2GRAY); + else { + CV_Assert(image.channels() == 1); + grayImage = image; + } + + // Use 32-bit integers if we won't overflow in the integral image + if ((image.depth() == CV_8U || image.depth() == CV_8S) && + (image.rows * image.cols) < 8388608 ) // 8388608 = 2 ^ (32 - 8(bit depth) - 1(sign bit)) + { + // Create the integral image appropriate for our type & usage + if (image.depth() == CV_8U) + computeDescriptors(grayImage, keypoints, _descriptors); + else if (image.depth() == CV_8S) + computeDescriptors(grayImage, keypoints, _descriptors); + else + CV_Error( Error::StsUnsupportedFormat, "" ); + } else { + // Create the integral image appropriate for our type & usage + if ( image.depth() == CV_8U ) + computeDescriptors(grayImage, keypoints, _descriptors); + else if ( image.depth() == CV_8S ) + computeDescriptors(grayImage, keypoints, _descriptors); + else if ( image.depth() == CV_16U ) + computeDescriptors(grayImage, keypoints, _descriptors); + else if ( image.depth() == CV_16S ) + computeDescriptors(grayImage, keypoints, _descriptors); + else + CV_Error( Error::StsUnsupportedFormat, "" ); + } +} + +template +void FREAK_Impl::extractDescriptor(srcMatType *pointsValue, void ** ptr) +{ + std::bitset** ptrScalar = (std::bitset**) ptr; + + // extracting descriptor preserving the order of SSE version + int cnt = 0; + for( int n = 7; n < FREAK::NB_PAIRS; n += 128) + { + for( int m = 8; m--; ) + { + int nm = n-m; + for(int kk = nm+15*8; kk >= nm; kk-=8, ++cnt) + { + (*ptrScalar)->set(kk, pointsValue[descriptionPairs[cnt].i] >= pointsValue[descriptionPairs[cnt].j]); + } + } + } + --(*ptrScalar); +} + +#if CV_SSE2 +template <> +void FREAK_Impl::extractDescriptor(uchar *pointsValue, void ** ptr) +{ + __m128i** ptrSSE = (__m128i**) ptr; + + // note that comparisons order is modified in each block (but first 128 comparisons remain globally the same-->does not affect the 128,384 bits segmanted matching strategy) + int cnt = 0; + for( int n = FREAK::NB_PAIRS/128; n-- ; ) + { + __m128i result128 = _mm_setzero_si128(); + for( int m = 128/16; m--; cnt += 16 ) + { + __m128i operand1 = _mm_set_epi8(pointsValue[descriptionPairs[cnt+0].i], + pointsValue[descriptionPairs[cnt+1].i], + pointsValue[descriptionPairs[cnt+2].i], + pointsValue[descriptionPairs[cnt+3].i], + pointsValue[descriptionPairs[cnt+4].i], + pointsValue[descriptionPairs[cnt+5].i], + pointsValue[descriptionPairs[cnt+6].i], + pointsValue[descriptionPairs[cnt+7].i], + pointsValue[descriptionPairs[cnt+8].i], + pointsValue[descriptionPairs[cnt+9].i], + pointsValue[descriptionPairs[cnt+10].i], + pointsValue[descriptionPairs[cnt+11].i], + pointsValue[descriptionPairs[cnt+12].i], + pointsValue[descriptionPairs[cnt+13].i], + pointsValue[descriptionPairs[cnt+14].i], + pointsValue[descriptionPairs[cnt+15].i]); + + __m128i operand2 = _mm_set_epi8(pointsValue[descriptionPairs[cnt+0].j], + pointsValue[descriptionPairs[cnt+1].j], + pointsValue[descriptionPairs[cnt+2].j], + pointsValue[descriptionPairs[cnt+3].j], + pointsValue[descriptionPairs[cnt+4].j], + pointsValue[descriptionPairs[cnt+5].j], + pointsValue[descriptionPairs[cnt+6].j], + pointsValue[descriptionPairs[cnt+7].j], + pointsValue[descriptionPairs[cnt+8].j], + pointsValue[descriptionPairs[cnt+9].j], + pointsValue[descriptionPairs[cnt+10].j], + pointsValue[descriptionPairs[cnt+11].j], + pointsValue[descriptionPairs[cnt+12].j], + pointsValue[descriptionPairs[cnt+13].j], + pointsValue[descriptionPairs[cnt+14].j], + pointsValue[descriptionPairs[cnt+15].j]); + + __m128i workReg = _mm_min_epu8(operand1, operand2); // emulated "not less than" for 8-bit UNSIGNED integers + workReg = _mm_cmpeq_epi8(workReg, operand2); // emulated "not less than" for 8-bit UNSIGNED integers + + workReg = _mm_and_si128(_mm_set1_epi16(short(0x8080 >> m)), workReg); // merge the last 16 bits with the 128bits std::vector until full + result128 = _mm_or_si128(result128, workReg); + } + (**ptrSSE) = result128; + ++(*ptrSSE); + } + (*ptrSSE) -= 8; +} +#endif + +template +void FREAK_Impl::computeDescriptors( InputArray _image, std::vector& keypoints, OutputArray _descriptors ){ + + Mat image = _image.getMat(); + Mat imgIntegral; + integral(image, imgIntegral, DataType::type); + std::vector kpScaleIdx(keypoints.size()); // used to save pattern scale index corresponding to each keypoints + const std::vector::iterator ScaleIdxBegin = kpScaleIdx.begin(); // used in std::vector erase function + const std::vector::iterator kpBegin = keypoints.begin(); // used in std::vector erase function + const float sizeCst = static_cast(FREAK::NB_SCALES/(FREAK_LOG2* nOctaves)); + srcMatType pointsValue[FREAK_NB_POINTS]; + int thetaIdx = 0; + int direction0; + int direction1; + + // compute the scale index corresponding to the keypoint size and remove keypoints close to the border + if( scaleNormalized ) + { + for( size_t k = keypoints.size(); k--; ) + { + //Is k non-zero? If so, decrement it and continue" + kpScaleIdx[k] = std::max( (int)(std::log(keypoints[k].size/FREAK_SMALLEST_KP_SIZE)*sizeCst+0.5) ,0); + if( kpScaleIdx[k] >= FREAK::NB_SCALES ) + kpScaleIdx[k] = FREAK::NB_SCALES-1; + + if( keypoints[k].pt.x <= patternSizes[kpScaleIdx[k]] || //check if the description at this specific position and scale fits inside the image + keypoints[k].pt.y <= patternSizes[kpScaleIdx[k]] || + keypoints[k].pt.x >= image.cols-patternSizes[kpScaleIdx[k]] || + keypoints[k].pt.y >= image.rows-patternSizes[kpScaleIdx[k]] + ) + { + keypoints.erase(kpBegin+k); + kpScaleIdx.erase(ScaleIdxBegin+k); + } + } + } + else + { + const int scIdx = std::max( cvRound(1.0986122886681*sizeCst) ,0); + for( size_t k = keypoints.size(); k--; ) + { + kpScaleIdx[k] = scIdx; // equivalent to the formule when the scale is normalized with a constant size of keypoints[k].size=3*SMALLEST_KP_SIZE + if( kpScaleIdx[k] >= FREAK::NB_SCALES ) + { + kpScaleIdx[k] = FREAK::NB_SCALES-1; + } + if( keypoints[k].pt.x <= patternSizes[kpScaleIdx[k]] || + keypoints[k].pt.y <= patternSizes[kpScaleIdx[k]] || + keypoints[k].pt.x >= image.cols-patternSizes[kpScaleIdx[k]] || + keypoints[k].pt.y >= image.rows-patternSizes[kpScaleIdx[k]] + ) + { + keypoints.erase(kpBegin+k); + kpScaleIdx.erase(ScaleIdxBegin+k); + } + } + } + + // allocate descriptor memory, estimate orientations, extract descriptors + if( !extAll ) + { + // extract the best comparisons only + _descriptors.create((int)keypoints.size(), FREAK::NB_PAIRS/8, CV_8U); + _descriptors.setTo(Scalar::all(0)); + Mat descriptors = _descriptors.getMat(); + + void *ptr = descriptors.data+(keypoints.size()-1)*descriptors.step[0]; + + for( size_t k = keypoints.size(); k--; ) { + // estimate orientation (gradient) + if( !orientationNormalized ) + { + thetaIdx = 0; // assign 0° to all keypoints + keypoints[k].angle = 0.0; + } + else + { + // get the points intensity value in the un-rotated pattern + for( int i = FREAK_NB_POINTS; i--; ) { + pointsValue[i] = meanIntensity(image, imgIntegral, + keypoints[k].pt.x, keypoints[k].pt.y, + kpScaleIdx[k], 0, i); + } + direction0 = 0; + direction1 = 0; + for( int m = 45; m--; ) + { + //iterate through the orientation pairs + const int delta = (pointsValue[ orientationPairs[m].i ]-pointsValue[ orientationPairs[m].j ]); + direction0 += delta*(orientationPairs[m].weight_dx)/2048; + direction1 += delta*(orientationPairs[m].weight_dy)/2048; + } + + keypoints[k].angle = static_cast(atan2((float)direction1,(float)direction0)*(180.0/CV_PI));//estimate orientation + + thetaIdx = cvRound(FREAK_NB_ORIENTATION*keypoints[k].angle*(1/360.0)); + + if( thetaIdx < 0 ) + thetaIdx += FREAK_NB_ORIENTATION; + + if( thetaIdx >= FREAK_NB_ORIENTATION ) + thetaIdx -= FREAK_NB_ORIENTATION; + } + // extract descriptor at the computed orientation + for( int i = FREAK_NB_POINTS; i--; ) { + pointsValue[i] = meanIntensity(image, imgIntegral, + keypoints[k].pt.x, keypoints[k].pt.y, + kpScaleIdx[k], thetaIdx, i); + } + + // Extract descriptor + extractDescriptor(pointsValue, &ptr); + } + } + else // extract all possible comparisons for selection + { + _descriptors.create((int)keypoints.size(), 128, CV_8U); + _descriptors.setTo(Scalar::all(0)); + Mat descriptors = _descriptors.getMat(); + std::bitset<1024>* ptr = (std::bitset<1024>*) (descriptors.data+(keypoints.size()-1)*descriptors.step[0]); + + for( size_t k = keypoints.size(); k--; ) + { + //estimate orientation (gradient) + if( !orientationNormalized ) + { + thetaIdx = 0;//assign 0° to all keypoints + keypoints[k].angle = 0.0; + } + else + { + //get the points intensity value in the un-rotated pattern + for( int i = FREAK_NB_POINTS;i--; ) + pointsValue[i] = meanIntensity(image, imgIntegral, + keypoints[k].pt.x,keypoints[k].pt.y, + kpScaleIdx[k], 0, i); + + direction0 = 0; + direction1 = 0; + for( int m = 45; m--; ) + { + //iterate through the orientation pairs + const int delta = (pointsValue[ orientationPairs[m].i ]-pointsValue[ orientationPairs[m].j ]); + direction0 += delta*(orientationPairs[m].weight_dx)/2048; + direction1 += delta*(orientationPairs[m].weight_dy)/2048; + } + + keypoints[k].angle = static_cast(atan2((float)direction1,(float)direction0)*(180.0/CV_PI)); //estimate orientation + + thetaIdx = cvRound(FREAK_NB_ORIENTATION*keypoints[k].angle*(1/360.0)); + + if( thetaIdx < 0 ) + thetaIdx += FREAK_NB_ORIENTATION; + + if( thetaIdx >= FREAK_NB_ORIENTATION ) + thetaIdx -= FREAK_NB_ORIENTATION; + } + // get the points intensity value in the rotated pattern + for( int i = FREAK_NB_POINTS; i--; ) { + pointsValue[i] = meanIntensity(image, imgIntegral, + keypoints[k].pt.x, keypoints[k].pt.y, + kpScaleIdx[k], thetaIdx, i); + } + + int cnt(0); + for( int i = 1; i < FREAK_NB_POINTS; ++i ) + { + //(generate all the pairs) + for( int j = 0; j < i; ++j ) + { + ptr->set(cnt, pointsValue[i] >= pointsValue[j] ); + ++cnt; + } + } + --ptr; + } + } +} + +// simply take average on a square patch, not even gaussian approx +template +imgType FREAK_Impl::meanIntensity( InputArray _image, InputArray _integral, + const float kp_x, + const float kp_y, + const unsigned int scale, + const unsigned int rot, + const unsigned int point) +{ + Mat image = _image.getMat(), integral = _integral.getMat(); + // get point position in image + const PatternPoint& FreakPoint = patternLookup[scale*FREAK_NB_ORIENTATION*FREAK_NB_POINTS + rot*FREAK_NB_POINTS + point]; + const float xf = FreakPoint.x+kp_x; + const float yf = FreakPoint.y+kp_y; + const int x = int(xf); + const int y = int(yf); + + // get the sigma: + const float radius = FreakPoint.sigma; + + // calculate output: + if( radius < 0.5 ) + { + // interpolation multipliers: + const int r_x = static_cast((xf-x)*1024); + const int r_y = static_cast((yf-y)*1024); + const int r_x_1 = (1024-r_x); + const int r_y_1 = (1024-r_y); + unsigned int ret_val; + // linear interpolation: + ret_val = r_x_1*r_y_1*int(image.at(y , x )) + + r_x *r_y_1*int(image.at(y , x+1)) + + r_x_1*r_y *int(image.at(y+1, x )) + + r_x *r_y *int(image.at(y+1, x+1)); + //return the rounded mean + ret_val += 2 * 1024 * 1024; + return static_cast(ret_val / (4 * 1024 * 1024)); + } + + // expected case: + + // calculate borders + const int x_left = cvRound(xf-radius); + const int y_top = cvRound(yf-radius); + const int x_right = cvRound(xf+radius+1);//integral image is 1px wider + const int y_bottom = cvRound(yf+radius+1);//integral image is 1px higher + iiType ret_val; + + ret_val = integral.at(y_bottom,x_right);//bottom right corner + ret_val -= integral.at(y_bottom,x_left); + ret_val += integral.at(y_top,x_left); + ret_val -= integral.at(y_top,x_right); + const int area = (x_right - x_left) * (y_bottom - y_top); + ret_val = (ret_val + area/2) / area; + //~ std::cout<(ret_val); +} + +// pair selection algorithm from a set of training images and corresponding keypoints +std::vector FREAK_Impl::selectPairs(const std::vector& images + , std::vector >& keypoints + , const double corrTresh + , bool verbose ) +{ + extAll = true; + // compute descriptors with all pairs + Mat descriptors; + + if( verbose ) + std::cout << "Number of images: " << images.size() << std::endl; + + for( size_t i = 0;i < images.size(); ++i ) + { + Mat descriptorsTmp; + compute(images[i],keypoints[i],descriptorsTmp); + descriptors.push_back(descriptorsTmp); + } + + if( verbose ) + std::cout << "number of keypoints: " << descriptors.rows << std::endl; + + //descriptor in floating point format (each bit is a float) + Mat descriptorsFloat = Mat::zeros(descriptors.rows, 903, CV_32F); + + std::bitset<1024>* ptr = (std::bitset<1024>*) (descriptors.data+(descriptors.rows-1)*descriptors.step[0]); + for( int m = descriptors.rows; m--; ) + { + for( int n = 903; n--; ) + { + if( ptr->test(n) == true ) + descriptorsFloat.at(m,n)=1.0f; + } + --ptr; + } + + std::vector pairStat; + for( int n = 903; n--; ) + { + // the higher the variance, the better --> mean = 0.5 + PairStat tmp = { fabs( mean(descriptorsFloat.col(n))[0]-0.5 ) ,n}; + pairStat.push_back(tmp); + } + + std::sort( pairStat.begin(),pairStat.end(), sortMean() ); + + std::vector bestPairs; + for( int m = 0; m < 903; ++m ) + { + if( verbose ) + std::cout << m << ":" << bestPairs.size() << " " << std::flush; + double corrMax(0); + + for( size_t n = 0; n < bestPairs.size(); ++n ) + { + int idxA = bestPairs[n].idx; + int idxB = pairStat[m].idx; + double corr(0); + // compute correlation between 2 pairs + corr = fabs(compareHist(descriptorsFloat.col(idxA), descriptorsFloat.col(idxB), HISTCMP_CORREL)); + + if( corr > corrMax ) + { + corrMax = corr; + if( corrMax >= corrTresh ) + break; + } + } + + if( corrMax < corrTresh/*0.7*/ ) + bestPairs.push_back(pairStat[m]); + + if( bestPairs.size() >= 512 ) + { + if( verbose ) + std::cout << m << std::endl; + break; + } + } + + std::vector idxBestPairs; + if( (int)bestPairs.size() >= FREAK::NB_PAIRS ) + { + for( int i = 0; i < FREAK::NB_PAIRS; ++i ) + idxBestPairs.push_back(bestPairs[i].idx); + } + else + { + if( verbose ) + std::cout << "correlation threshold too small (restrictive)" << std::endl; + CV_Error(Error::StsError, "correlation threshold too small (restrictive)"); + } + extAll = false; + return idxBestPairs; +} + + +/* +// create an image showing the brisk pattern +void FREAKImpl::drawPattern() +{ + Mat pattern = Mat::zeros(1000, 1000, CV_8UC3) + Scalar(255,255,255); + int sFac = 500 / patternScale; + for( int n = 0; n < kNB_POINTS; ++n ) + { + PatternPoint& pt = patternLookup[n]; + circle(pattern, Point( pt.x*sFac,pt.y*sFac)+Point(500,500), pt.sigma*sFac, Scalar(0,0,255),2); + // rectangle(pattern, Point( (pt.x-pt.sigma)*sFac,(pt.y-pt.sigma)*sFac)+Point(500,500), Point( (pt.x+pt.sigma)*sFac,(pt.y+pt.sigma)*sFac)+Point(500,500), Scalar(0,0,255),2); + + circle(pattern, Point( pt.x*sFac,pt.y*sFac)+Point(500,500), 1, Scalar(0,0,0),3); + std::ostringstream oss; + oss << n; + putText( pattern, oss.str(), Point( pt.x*sFac,pt.y*sFac)+Point(500,500), FONT_HERSHEY_SIMPLEX,0.5, Scalar(0,0,0), 1); + } + imshow( "FreakDescriptorExtractor pattern", pattern ); + waitKey(0); +} +*/ + +// ------------------------------------------------- +/* FREAK interface implementation */ +FREAK_Impl::FREAK_Impl( bool _orientationNormalized, bool _scaleNormalized + , float _patternScale, int _nOctaves, const std::vector& _selectedPairs ) + : orientationNormalized(_orientationNormalized), scaleNormalized(_scaleNormalized), + patternScale(_patternScale), nOctaves(_nOctaves), extAll(false), + patternScale0(0.0), nOctaves0(0), selectedPairs0(_selectedPairs) +{ +} + +FREAK_Impl::~FREAK_Impl() +{ +} + +int FREAK_Impl::descriptorSize() const +{ + return FREAK::NB_PAIRS / 8; // descriptor length in bytes +} + +int FREAK_Impl::descriptorType() const +{ + return CV_8U; +} + +int FREAK_Impl::defaultNorm() const +{ + return NORM_HAMMING; +} + +Ptr FREAK::create(bool orientationNormalized, + bool scaleNormalized, + float patternScale, + int nOctaves, + const std::vector& selectedPairs) +{ + return makePtr(orientationNormalized, scaleNormalized, + patternScale, nOctaves, selectedPairs); +} + +} +} // END NAMESPACE CV diff --git a/kernel/Freak c++ files/preprocessing_befor_kernel.cpp b/kernel/Freak c++ files/preprocessing_befor_kernel.cpp new file mode 100644 index 0000000..143c2dd --- /dev/null +++ b/kernel/Freak c++ files/preprocessing_befor_kernel.cpp @@ -0,0 +1,172 @@ +static const float FREAK_LOG2 = 0.693147180559945; +static const int FREAK_NB_ORIENTATION = 256; +static const int NB_ORIENPAIRS = 45; +static const int FREAK_NB_POINTS = 43; +static const int FREAK_SMALLEST_KP_SIZE = 7; // smallest size of keypoints +static const int NB_PAIRS = 512; +static const int NB_SCALES = 64; + +bool orientationNormalized = true; +bool scaleNormalized = true; +float patternScale = 22.0f; +int nOctaves = 4; +//float patternScale0 = 0.0; +//int nOctaves0 = 0; +bool extAll = false; // true if all pairs need to be extracted for pairs selection +//int* selectedPairs0; + +float3 patternLookup[NB_SCALES * FREAK_NB_ORIENTATION * FREAK_NB_POINTS]; // look-up table for the pattern points (position+sigma of all points at all scales and orientation) +int patternSizes[NB_SCALES]; // size of the pattern at a specific scale (used to check if a point is within image boundaries) +uchar2 descriptionPairs[NB_PAIRS]; +int4 orientationPairs[NB_ORIENPAIRS]; + +// default pairs +static const int FREAK_DEF_PAIRS[NB_PAIRS] = + { + 404,431,818,511,181,52,311,874,774,543,719,230,417,205,11, + 560,149,265,39,306,165,857,250,8,61,15,55,717,44,412, + 592,134,761,695,660,782,625,487,549,516,271,665,762,392,178, + 796,773,31,672,845,548,794,677,654,241,831,225,238,849,83, + 691,484,826,707,122,517,583,731,328,339,571,475,394,472,580, + 381,137,93,380,327,619,729,808,218,213,459,141,806,341,95, + 382,568,124,750,193,749,706,843,79,199,317,329,768,198,100, + 466,613,78,562,783,689,136,838,94,142,164,679,219,419,366, + 418,423,77,89,523,259,683,312,555,20,470,684,123,458,453,833, + 72,113,253,108,313,25,153,648,411,607,618,128,305,232,301,84, + 56,264,371,46,407,360,38,99,176,710,114,578,66,372,653, + 129,359,424,159,821,10,323,393,5,340,891,9,790,47,0,175,346, + 236,26,172,147,574,561,32,294,429,724,755,398,787,288,299, + 769,565,767,722,757,224,465,723,498,467,235,127,802,446,233, + 544,482,800,318,16,532,801,441,554,173,60,530,713,469,30, + 212,630,899,170,266,799,88,49,512,399,23,500,107,524,90, + 194,143,135,192,206,345,148,71,119,101,563,870,158,254,214, + 276,464,332,725,188,385,24,476,40,231,620,171,258,67,109, + 844,244,187,388,701,690,50,7,850,479,48,522,22,154,12,659, + 736,655,577,737,830,811,174,21,237,335,353,234,53,270,62, + 182,45,177,245,812,673,355,556,612,166,204,54,248,365,226, + 242,452,700,685,573,14,842,481,468,781,564,416,179,405,35, + 819,608,624,367,98,643,448,2,460,676,440,240,130,146,184, + 185,430,65,807,377,82,121,708,239,310,138,596,730,575,477, + 851,797,247,27,85,586,307,779,326,494,856,324,827,96,748, + 13,397,125,688,702,92,293,716,277,140,112,4,80,855,839,1, + 413,347,584,493,289,696,19,751,379,76,73,115,6,590,183,734, + 197,483,217,344,330,400,186,243,587,220,780,200,793,246,824, + 41,735,579,81,703,322,760,720,139,480,490,91,814,813,163, + 152,488,763,263,425,410,576,120,319,668,150,160,302,491,515, + 260,145,428,97,251,395,272,252,18,106,358,854,485,144,550, + 131,133,378,68,102,104,58,361,275,209,697,582,338,742,589, + 325,408,229,28,304,191,189,110,126,486,211,547,533,70,215, + 670,249,36,581,389,605,331,518,442,822 + }; + +float scaleStep = pow(2.0, (float)(nOctaves)/FREAK_NB_SCALES ); // 2 ^ ( (nOctaves-1) /nbScales) + +const int n[8] = {6, 6, 6, 6, 6, 6, 6, 1}; // number of points on each concentric circle (from outer to inner) +const float bigR = (float)(2.0 / 3.0); // bigger radius +const float smallR = (float)(2.0 / 24.0); // smaller radius +const float unitSpace = (float)((bigR - smallR) / 21.0 ); // define spaces between concentric circles (from center to outer: 1,2,3,4,5,6) +// radii of the concentric cirles (from outer to inner) +const float radius[8] = {bigR , bigR - 6 * unitSpace, bigR - 11 * unitSpace, bigR - 15 * unitSpace, + bigR - 18 * unitSpace, bigR - 20 * unitSpace, smallR , 0.0}; +// sigma of pattern points (each group of 6 points on a concentric cirle has the same sigma) +const float sigma[8] = {radius[0] / 2.0, radius[1] / 2.0, radius[2] / 2.0, radius[3] / 2.0, + radius[4] / 2.0, radius[5] / 2.0, radius[6] / 2.0, radius[6] / 2.0}; + +for( int scaleIdx=0; scaleIdx < NB_SCALES; ++scaleIdx ) +{ +patternSizes[scaleIdx] = 0; // proper initialization +float scalingFactor = pow(scaleStep,scaleIdx); //scale of the pattern, scaleStep ^ scaleIdx + +for( int orientationIdx = 0; orientationIdx < FREAK_NB_ORIENTATION; ++orientationIdx ) +{ +int pointIdx = 0; + +for( size_t i = 0; i < 8; ++i ) +{ +for( int k = 0 ; k < n[i]; ++k ) +{ +float alpha = (float)(k) * 2 * (float)3.141592653589793238462643383279502884197169399375 / (float)(n[i]) + + ((float)3.141592653589793238462643383279502884197169399375 / n[i] * ( i % 2 )/*beta*/) + + ((float)(orientationIdx * 2 * (float)3.141592653589793238462643383279502884197169399375 / (float)(FREAK_NB_ORIENTATION)) /* theta*/); + +// add the point to the look-up table +float3* point = patternLookup[ scaleIdx * FREAK_NB_ORIENTATION * FREAK_NB_POINTS + orientationIdx * FREAK_NB_POINTS + pointIdx]; +point->x = (float)(radius[i] * cos(alpha) * scalingFactor * patternScale); //x +point->y = (float)(radius[i] * sin(alpha) * scalingFactor * patternScale); //y +point->z = (float)(sigma[i] * scalingFactor * patternScale); //sigma + +// adapt the sizeList if necessary +const int sizeMax = (int)(ceil((radius[i] + sigma[i]) * scalingFactor * patternScale)) + 1; +if( patternSizes[scaleIdx] < sizeMax ) +patternSizes[scaleIdx] = sizeMax; + +++pointIdx; +} +} +} +} + +// build the list of orientation pairs +orientationPairs[0].w=0; orientationPairs[0].x=3; orientationPairs[1].w=1; orientationPairs[1].x=4; orientationPairs[2].w=2; orientationPairs[2].x=5; +orientationPairs[3].w=0; orientationPairs[3].x=2; orientationPairs[4].w=1; orientationPairs[4].x=3; orientationPairs[5].w=2; orientationPairs[5].x=4; +orientationPairs[6].w=3; orientationPairs[6].x=5; orientationPairs[7].w=4; orientationPairs[7].x=0; orientationPairs[8].w=5; orientationPairs[8].x=1; + +orientationPairs[9].w=6; orientationPairs[9].x=9; orientationPairs[10].w=7; orientationPairs[10].x=10; orientationPairs[11].w=8; orientationPairs[11].x=11; +orientationPairs[12].w=6; orientationPairs[12].x=8; orientationPairs[13].w=7; orientationPairs[13].x=9; orientationPairs[14].w=8; orientationPairs[14].x=10; +orientationPairs[15].w=9; orientationPairs[15].x=11; orientationPairs[16].w=10; orientationPairs[16].x=6; orientationPairs[17].w=11; orientationPairs[17].x=7; + +orientationPairs[18].w=12; orientationPairs[18].x=15; orientationPairs[19].w=13; orientationPairs[19].x=16; orientationPairs[20].w=14; orientationPairs[20].x=17; +orientationPairs[21].w=12; orientationPairs[21].x=14; orientationPairs[22].w=13; orientationPairs[22].x=15; orientationPairs[23].w=14; orientationPairs[23].x=16; +orientationPairs[24].w=15; orientationPairs[24].x=17; orientationPairs[25].w=16; orientationPairs[25].x=12; orientationPairs[26].w=17; orientationPairs[26].x=13; + +orientationPairs[27].w=18; orientationPairs[27].x=21; orientationPairs[28].w=19; orientationPairs[28].x=22; orientationPairs[29].w=20; orientationPairs[29].x=23; +orientationPairs[30].w=18; orientationPairs[30].x=20; orientationPairs[31].w=19; orientationPairs[31].x=21; orientationPairs[32].w=20; orientationPairs[32].x=22; +orientationPairs[33].w=21; orientationPairs[33].x=23; orientationPairs[34].w=22; orientationPairs[34].x=18; orientationPairs[35].w=23; orientationPairs[35].x=19; + +orientationPairs[36].w=24; orientationPairs[36].x=27; orientationPairs[37].w=25; orientationPairs[37].x=28; orientationPairs[38].w=26; orientationPairs[38].x=29; +orientationPairs[39].w=30; orientationPairs[39].x=33; orientationPairs[40].w=31; orientationPairs[40].x=34; orientationPairs[41].w=32; orientationPairs[41].x=35; +orientationPairs[42].w=36; orientationPairs[42].x=39; orientationPairs[43].w=37; orientationPairs[43].x=40; orientationPairs[44].w=38; orientationPairs[44].x=41; + +for( unsigned m = NB_ORIENPAIRS; m--; ) +{ +float dx = patternLookup[orientationPairs[m].w].x - patternLookup[orientationPairs[m].x].x; +float dy = patternLookup[orientationPairs[m].w].y - patternLookup[orientationPairs[m].x].y; +float norm_sq = (dx * dx + dy * dy); +orientationPairs[m].y = round((dx / (norm_sq)) * 4096.0); //weight_dx +orientationPairs[m].z = round((dy / (norm_sq)) * 4096.0); //weight_dy +} + +// build the list of description pairs +uchar2 allPairs[(int)((FREAK_NB_POINTS * (FREAK_NB_POINTS + 1)) / 2)]; +for( int i = 1, idx = 0; i < FREAK_NB_POINTS; ++i ) +{ +// (generate all the pairs) +for( int j = 0; j < i; ++j ) +{ +allPairs[idx++] = (uchar2){(uchar)i, (uchar)j}; +} +} +for( int i = 0; i < NB_PAIRS; ++i ) +descriptionPairs[i] = allPairs[FREAK_DEF_PAIRS[i]]; + +int kpScaleIdx[keypoints.size()]; + +// kernel running with parameter : +// __kernel void freak( +// __read_only image2d_t image, // grayescale image - height X width +// __read_only image2d_t imgIntegral, // (height+1) X (width+1) with zero values +// __global float3* keypoints, // ptx, pty, angle +// __global uchar* descriptors, // kp_size * (64 = NB_PAIRS/8) - CV_8U with zero values +// __global int* patternSizes, // array size : NB_SCALES = 64 +// __global int4* orientationPairs, // array size : FREAK_NB_ORIENTATION = 256 +// __global float3* patternLookup, // array size : NB_SCALES(64) * FREAK_NB_ORIENTATION(256) * FREAK_NB_POINTS(43) +// __global uchar2* descriptionPairs, // array size : NB_PAIRS = 512 +// __global int* nOctaves, +// __global int* kpScaleIdx //array size = kp_size - used to save pattern scale index corresponding to each keypoints +// ) +// FREAK_LOG2 = 0.693147180559945; +// FREAK_NB_ORIENTATION = 256; +// FREAK_NB_POINTS = 43; +// FREAK_SMALLEST_KP_SIZE = 7; +// NB_PAIRS = 512; +// NB_SCALES = 64; \ No newline at end of file diff --git a/kernel/compute-residual.cl b/kernel/compute-residual.cl new file mode 100644 index 0000000..5afa6a7 --- /dev/null +++ b/kernel/compute-residual.cl @@ -0,0 +1,165 @@ +// Copyright (C) 2021 Majid Geravand +// Copyright (C) 2021 Gfuse + +// Enable OpenCL 32-bit integer atomic functions. +#pragma OPENCL EXTENSION cl_khr_global_int32_base_atomics : enable + +float2 world2cam(float3 feature) +{ + float r = sqrt(pow(feature.x/feature.z, 2) + pow(feature.y/feature.z, 2)); + float factor = 1.0; + if((float)S != 0 || r > 0.001) + factor = (atan(r * 2.0 * tan(0.5 * (float)S)) / (r * (float)S)); + return (float2)((float)C_X + (float)F_X * factor * feature.x/feature.z, (float)C_Y + (float)F_Y * factor * feature.y/feature.z); +} + +float3 xyz_cur(float3 cur, float3 ref, float3 ref_feature) +{ + if(ref.z < 0.0) + ref.z = 3.141592653589793238462643383279502884197169399375 - ref.z; + else + ref.z = ref.z - 3.1415926535897932384626433832795028841971693993; + float cc_ss = cos(cur.z) * cos(ref.z) - sin(cur.z) * sin(ref.z); + float sc_cs = cos(cur.z) * sin(ref.z) + cos(ref.z) * sin(cur.z); + return (float3)(cc_ss * ref_feature.x + sc_cs * ref_feature.z + cur.x - ref.x * cos(cur.z) - ref.z * sin(cur.z), + ref_feature.y, + -1.0 * sc_cs * ref_feature.x + cc_ss * ref_feature.z + cur.z + ref.x * sin(cur.z) - ref.z * cos(cur.z)); +} + +void jacobian_xyz2uv(float3 xyz_in_f, float* J) +{ + *(J + 0) = -(1. / xyz_in_f[2]); // -1/z + *(J + 1) = (xyz_in_f[0]) * ((1. / xyz_in_f[2]) * (1. / xyz_in_f[2])); // x/z^2 + *(J + 2) = -(1.0 + pow((xyz_in_f[0]),2) / ((1. / xyz_in_f[2]) * (1. / xyz_in_f[2]))); // -(1.0 + x^2/z^2) + *(J + 3) = 1e-19; // 0 + *(J + 4) = (xyz_in_f[1]) * ((1. / xyz_in_f[2]) * (1. / xyz_in_f[2])); // y/z^2 + *(J + 5) = -(xyz_in_f[0]) * (xyz_in_f[1]) / ((1. / xyz_in_f[2]) * (1. / xyz_in_f[2])); // -x*y/z^2 +} + +void jacobian_xyz2uv_2(float3 xyz_in_f, float3 ref_pose, float3 cur_pose, float* J) +{ float3 txtytetha = cur_pose - ref_pose; + float2 uv = world2cam(xyz_in_f); + *(J + 0) = 1.0; + *(J + 1) = 0.0; + *(J + 2) = ((-1 * sin(txtytetha[2]) * cos(ref_pose[2])) - (cos(txtytetha[2]) * sin(ref_pose[2]))) * uv[0] + ((cos(txtytetha[2]) * cos(ref_pose[2])) - (sin(txtytetha[2]) * sin(ref_pose[2]))) * uv[1]; + *(J + 3) = 0.0; + *(J + 4) = 1.0; + *(J + 5) = ((-1 * cos(txtytetha[2]) * cos(ref_pose[2])) + (sin(txtytetha[2]) * sin(ref_pose[2]))) * uv[0] - ((sin(txtytetha[2]) * cos(ref_pose[2])) + (cos(txtytetha[2]) * sin(ref_pose[2]))) * uv[1]; +} + +void compute_hessain(float3 j, __global float* H, float w) +{ + *(H + 0) += j[0] * j[0] * w; + *(H + 1) += j[0] * j[1] * w; + *(H + 2) += j[0] * j[2] * w; + *(H + 3) += j[1] * j[0] * w; + *(H + 4) += j[1] * j[1] * w; + *(H + 5) += j[1] * j[2] * w; + *(H + 6) += j[2] * j[0] * w; + *(H + 7) += j[2] * j[1] * w; + *(H + 8) += j[2] * j[2] * w; +} + +__kernel void compute_residual( + __read_only image2d_t image_cur, // current frame + __read_only image2d_t image_ref, // reference frame + __global float3 * cur_pose,//[reference frame pose{x,z,pitch}] + __global float3 * ref_pose,//[current frame pose{x,z,pitch}] + __global float3 * ref_feature, // feature on the reference frame, when we applied the distance calculation: xyz_ref((*it)->f*((*it)->point->pos_ - Eigen::Vector3d(ref_pos[0],0.0,ref_pos[1])).norm()); + __global float2 * featue_px, + int level, + __global float * errors, + __global float * Hessian, + __global float3 * Jacobian, + __global float * chi, + float scale_ +) +{ + float scale = pow(2.0, -level); + // Prepare a suitable OpenCL image sampler. + sampler_t const sampler = CLK_ADDRESS_CLAMP | CLK_FILTER_NEAREST; + // Use global work feature. + int f = get_global_id(0); + // check if reference with patch size is within image + float2 uv_ref = featue_px[f] * scale; + float2 uv_ref_i = floor(uv_ref); + if(uv_ref_i.x - (PATCH_HALFSIZE + 1) < 0 || uv_ref_i.y - (PATCH_HALFSIZE + 1) < 0 || uv_ref_i.x + (PATCH_HALFSIZE + 1) >= get_image_dim(image_ref)[0] || uv_ref_i.y + (PATCH_HALFSIZE + 1) >= get_image_dim(image_ref)[1])return; + // evaluate projection jacobian + float frame_jac[6]={0.0}; // 2X3 + jacobian_xyz2uv(sqrt(pow(ref_feature[f] - (float3)(ref_pose[0].x,0.0,ref_pose[0].y), 2.0)), &frame_jac); + //jacobian_xyz2uv_2(ref_feature[f], ref_pose[0], cur_pose[0], &frame_jac); + //printf("jacobian : %f %f %f %f %f %f\n", frame_jac[0], frame_jac[1], frame_jac[2], frame_jac[3], frame_jac[4], frame_jac[5]); + // compute bilateral interpolation weights for reference image + float2 subpix= uv_ref - uv_ref_i; + float w_ref_tl = (1.0 - subpix.x) * (1.0 - subpix.y); + float w_ref_tr = subpix.x * (1.0 - subpix.y); + float w_ref_bl = (1.0 - subpix.x) * subpix.y; + float w_ref_br = subpix.x * subpix.y; + + // compute bilateral interpolation weights for the current image + float2 uv_cur_pyr = world2cam(xyz_cur(cur_pose[0], ref_pose[0] ,ref_feature[f])) * scale; + float2 uv_cur_i = floor(uv_cur_pyr); + float2 subpix_uv_cur = uv_cur_pyr - uv_cur_i; + float w_cur_tl = (1.0 - subpix_uv_cur.x) * (1.0 - subpix_uv_cur.y); + float w_cur_tr = subpix_uv_cur.x * (1.0 - subpix_uv_cur.y); + float w_cur_bl = (1.0 - subpix_uv_cur.x) * subpix_uv_cur.y; + float w_cur_br = subpix_uv_cur.x * subpix_uv_cur.y; + float e = 0.0; + float chi_=0.0; + for(int y = 0; y < PATCH_SIZE; ++y) + { + int ref_element_addr = (int)(uv_ref_i.y + y - PATCH_HALFSIZE) * get_image_dim(image_ref)[0] + (uv_ref_i.x - PATCH_HALFSIZE); + int cur_element_addr = (int)(uv_cur_i.y + y - PATCH_HALFSIZE) * get_image_dim(image_cur)[0] + (uv_cur_i.x - PATCH_HALFSIZE); + + for(int x = 0; x < PATCH_SIZE; ++x, ++ref_element_addr, ++cur_element_addr) + { + // precompute interpolated reference patch color + int2 px_reftl = (int2)( ref_element_addr % get_image_dim(image_ref)[0], ref_element_addr / get_image_dim(image_ref)[0]); + int2 px_reftr = (int2)((ref_element_addr + 1) % get_image_dim(image_ref)[0], (ref_element_addr + 1) / get_image_dim(image_ref)[0]); + int2 px_refbl = (int2)((ref_element_addr + get_image_dim(image_ref)[0]) % get_image_dim(image_ref)[0], (ref_element_addr + get_image_dim(image_ref)[0]) / get_image_dim(image_ref)[0]); + int2 px_refbr = (int2)((ref_element_addr + get_image_dim(image_ref)[0] + 1) % get_image_dim(image_ref)[0], (ref_element_addr + get_image_dim(image_ref)[0] + 1) / get_image_dim(image_ref)[0]); + float value = w_ref_tl * read_imageui(image_ref, sampler, px_reftl).x + w_ref_tr * read_imageui(image_ref, sampler, px_reftr).x + + w_ref_bl * read_imageui(image_ref, sampler, px_refbl).x + w_ref_br * read_imageui(image_ref, sampler, px_refbr).x; + + float dx = 0.5f * ((w_ref_tl * read_imageui(image_ref, sampler, (int2) ((ref_element_addr + 1) % get_image_dim(image_ref)[0], ((ref_element_addr + 1) / get_image_dim(image_ref)[0]))).x + + w_ref_tr * read_imageui(image_ref, sampler, (int2) ((ref_element_addr + 2) % get_image_dim(image_ref)[0], ((ref_element_addr + 2) / get_image_dim(image_ref)[0]))).x + + w_ref_bl * read_imageui(image_ref, sampler, (int2) ((ref_element_addr + (get_image_dim(image_ref)[0]) + 1) % get_image_dim(image_ref)[0], ((ref_element_addr + (get_image_dim(image_ref)[0]) + 1) / get_image_dim(image_ref)[0]))).x + + w_ref_br * read_imageui(image_ref, sampler, (int2) ((ref_element_addr + (get_image_dim(image_ref)[0]) + 2) % get_image_dim(image_ref)[0], ((ref_element_addr + (get_image_dim(image_ref)[0]) + 2) / get_image_dim(image_ref)[0]))).x) + -(w_ref_tl * read_imageui(image_ref, sampler, (int2) ((ref_element_addr - 1) % get_image_dim(image_ref)[0], ((ref_element_addr - 1) / get_image_dim(image_ref)[0]))).x + + w_ref_tr * read_imageui(image_ref, sampler, (int2) ((ref_element_addr + 0) % get_image_dim(image_ref)[0], ((ref_element_addr + 0) / get_image_dim(image_ref)[0]))).x + + w_ref_bl * read_imageui(image_ref, sampler, (int2) ((ref_element_addr + (get_image_dim(image_ref)[0]) - 1) % get_image_dim(image_ref)[0], ((ref_element_addr + (get_image_dim(image_ref)[0]) - 1) / get_image_dim(image_ref)[0]))).x + + w_ref_br * read_imageui(image_ref, sampler, (int2) ((ref_element_addr + (get_image_dim(image_ref)[0])) % get_image_dim(image_ref)[0], ((ref_element_addr + (get_image_dim(image_ref)[0])) / get_image_dim(image_ref)[0]))).x)); + + float dy = 0.5f * ((w_ref_tl * read_imageui(image_ref, sampler, (int2) ((ref_element_addr + (get_image_dim(image_ref)[0])) % get_image_dim(image_ref)[0], ((ref_element_addr + (get_image_dim(image_ref)[0])) / get_image_dim(image_ref)[0]))).x + + w_ref_tr * read_imageui(image_ref, sampler, (int2) ((ref_element_addr + (get_image_dim(image_ref)[0]) + 1) % get_image_dim(image_ref)[0], ((ref_element_addr + (get_image_dim(image_ref)[0]) + 1) / get_image_dim(image_ref)[0]))).x + + w_ref_bl * read_imageui(image_ref, sampler, (int2) ((ref_element_addr + (get_image_dim(image_ref)[0]) * 2) % get_image_dim(image_ref)[0], ((ref_element_addr + (get_image_dim(image_ref)[0]) * 2) / get_image_dim(image_ref)[0]))).x + + w_ref_br * read_imageui(image_ref, sampler, (int2) ((ref_element_addr + (get_image_dim(image_ref)[0]) * 2 + 1) % get_image_dim(image_ref)[0], ((ref_element_addr + (get_image_dim(image_ref)[0]) * 2 + 1) / get_image_dim(image_ref)[0]))).x) + -(w_ref_tl * read_imageui(image_ref, sampler, (int2) ((ref_element_addr + (-get_image_dim(image_ref)[0])) % get_image_dim(image_ref)[0], ((ref_element_addr + (-get_image_dim(image_ref)[0])) / get_image_dim(image_ref)[0]))).x + + w_ref_tr * read_imageui(image_ref, sampler, (int2) ((ref_element_addr + (1 - get_image_dim(image_ref)[0])) % get_image_dim(image_ref)[0], ((ref_element_addr + (1 - get_image_dim(image_ref)[0])) / get_image_dim(image_ref)[0]))).x + + w_ref_bl * read_imageui(image_ref, sampler, (int2) ((ref_element_addr + 0) % get_image_dim(image_ref)[0], ((ref_element_addr + 0) / get_image_dim(image_ref)[0]))).x + + w_ref_br * read_imageui(image_ref, sampler, (int2) ((ref_element_addr + 1) % get_image_dim(image_ref)[0], ((ref_element_addr + 1) / get_image_dim(image_ref)[0]))).x)); + + // compute residual + int2 px_curtl = (int2)( cur_element_addr % get_image_dim(image_cur)[0], cur_element_addr / get_image_dim(image_cur)[0]); + int2 px_curtr = (int2)((cur_element_addr + 1) % get_image_dim(image_cur)[0], (cur_element_addr + 1) / get_image_dim(image_cur)[0]); + int2 px_curbl = (int2)((cur_element_addr + (get_image_dim(image_cur)[0])) % get_image_dim(image_cur)[0], (cur_element_addr + (get_image_dim(image_cur)[0])) / get_image_dim(image_cur)[0]); + int2 px_curbr = (int2)((cur_element_addr + (get_image_dim(image_cur)[0]) + 1) % get_image_dim(image_cur)[0], (cur_element_addr + (get_image_dim(image_cur)[0]) + 1) / get_image_dim(image_cur)[0]); + + float res = value - w_cur_tl * read_imageui(image_cur, sampler, px_curtl).x + w_cur_tr * read_imageui(image_cur, sampler, px_curtr).x + + w_cur_bl * read_imageui(image_cur, sampler, px_curbl).x + w_cur_br * read_imageui(image_cur, sampler, px_curbr).x; + // used to compute scale for robust cost + e += fabs(res); + // robustification + float weight = res/scale_; //1.48f * vk::getMedian(errors) + chi_ += pow(res,2) * weight; + float3 j_row0 = (float3)((dx * frame_jac[0]), (dx * frame_jac[1]), (dx * frame_jac[2])); + float3 j_row1 = (float3)((dy * frame_jac[3]), (dy * frame_jac[4]), (dy * frame_jac[5])); + float3 J = (j_row0 + j_row1) * ((float)F_X / scale); + compute_hessain(J, &Hessian[f*9], weight); + Jacobian[f] -= J * res * weight; + +} +} +errors[f] = e / pow(PATCH_SIZE,2.0); +chi[f]=chi_; +} diff --git a/kernel/freak.cl b/kernel/freak.cl new file mode 100644 index 0000000..498e29f --- /dev/null +++ b/kernel/freak.cl @@ -0,0 +1,129 @@ +void meanIntensity( image2d_t _image, image2d_t _integral, float kp_x, float kp_y, + int scale, int rot, int* point, float3* patternLookupPtr, uchar* output) +{ + // Prepare a suitable OpenCL image sampler. + sampler_t const sampler = CLK_ADDRESS_CLAMP | CLK_FILTER_NEAREST; + // get point position in image + float3* FreakPoint = patternLookupPtr + (scale * FREAK_NB_ORIENTATION * FREAK_NB_POINTS + rot * FREAK_NB_POINTS + (*point)); + float xf = FreakPoint->x + kp_x; + float yf = FreakPoint->y + kp_y; + int x = (int)xf; + int y = (int)yf; + + // get the sigma: + float radius = FreakPoint->z;//sigma + + // calculate output: + if( radius < 0.5 ) + { + // interpolation multipliers: + const int r_x = (int)((xf - x) * 1024); + const int r_y = (int)((yf - y) * 1024); + const int r_x_1 = (1024 - r_x); + const int r_y_1 = (1024 - r_y); + uint ret_val; + // linear interpolation: + ret_val = (r_x_1 * r_y_1 * (int)(read_imageui(_image, sampler, (int2)(y , x ))).x) + + (r_x * r_y_1 * (int)(read_imageui(_image, sampler, (int2)(y , x+1))).x) + + (r_x_1 * r_y * (int)(read_imageui(_image, sampler, (int2)(y+1, x ))).x) + + (r_x * r_y * (int)(read_imageui(_image, sampler, (int2)(y+1, x+1))).x); + // return the rounded mean + ret_val += 2 * 1024 * 1024; + output[*point] = (uchar)(ret_val / (4 * 1024 * 1024)); + } + + // calculate borders + const int x_left = round(xf - radius); + const int y_top = round(yf - radius); + const int x_right = round(xf + radius + 1); // integral image is 1px wider + const int y_bottom = round(yf + radius + 1); // integral image is 1px higher + int ret_val; + + ret_val = read_imageui(_integral, sampler, (int2)(y_bottom, x_right)).x; // bottom right corner + ret_val -= read_imageui(_integral, sampler, (int2)(y_bottom, x_left)).x; // bottom left corner + ret_val += read_imageui(_integral, sampler, (int2)(y_top, x_left)).x; // top left corner + ret_val -= read_imageui(_integral, sampler, (int2)(y_top, x_right)).x; // top right corner + const int area = (x_right - x_left) * (y_bottom - y_top); + ret_val = (ret_val + area / 2) / area; + output[*point] = (uchar)(ret_val); +} +//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// +__kernel void freak( + __read_only image2d_t image, // grayescale image + __read_only image2d_t imgIntegral, + __global float3* keypoints, // ptx, pty, angle + __global uchar* descriptors, // kp_size * (64 = NB_PAIRS/8) - CV_8U with zero values + __global int* patternSizes, // array size : NB_SCALES = 64 + __global int4* orientationPairs, // array size : FREAK_NB_ORIENTATION = 256 + __global float3* patternLookup, // array size : NB_SCALES(64) * FREAK_NB_ORIENTATION(256) * FREAK_NB_POINTS(43) + __global uchar2* descriptionPairs, // array size : NB_PAIRS = 512 + __global int* nOctaves, + __global int* kpScaleIdx //array size = kp_size - used to save pattern scale index corresponding to each keypoints + ) +{ + const float sizeCst = (float)(NB_SCALES / (FREAK_LOG2 * *nOctaves)); + uchar pointsValue[FREAK_NB_POINTS]; + int thetaIdx = 0; + int direction0; + int direction1; + + int k = get_global_id(0); + + kpScaleIdx[k] = max( (int)(log(7.0 / FREAK_SMALLEST_KP_SIZE) * sizeCst + 0.5) ,0); + if( kpScaleIdx[k] >= NB_SCALES ) + kpScaleIdx[k] = NB_SCALES - 1; + + if( keypoints[k].x <= patternSizes[kpScaleIdx[k]] || // check if the description at this specific position and scale fits inside the image + keypoints[k].y <= patternSizes[kpScaleIdx[k]] || + keypoints[k].x >= get_image_dim(image).y - patternSizes[kpScaleIdx[k]] || + keypoints[k].y >= get_image_dim(image).x - patternSizes[kpScaleIdx[k]]) + return; + + // get the points intensity value in the un-rotated pattern + for( int i = FREAK_NB_POINTS; i--; ) + { + meanIntensity(image, imgIntegral, keypoints[k].x, keypoints[k].y, + *(kpScaleIdx + k), 0, &i, patternLookup, pointsValue); + } + direction0 = 0; + direction1 = 0; + for( int m = 45; m--; ) + { + // iterate through the orientation pairs + const int delta = (pointsValue[ orientationPairs[m].w ] - pointsValue[ orientationPairs[m].x ]); + direction0 += delta * (orientationPairs[m].y) / 2048; // weight_dx + direction1 += delta * (orientationPairs[m].z) / 2048; // weight_dy + } + + thetaIdx = round(FREAK_NB_ORIENTATION * + (float)(atan2((float)direction1,(float)direction0) * (180.0 / 3.141592653589793238462643383279502884197169399375)) * + (1 / 360.0)); + + if( thetaIdx < 0 ) + thetaIdx += FREAK_NB_ORIENTATION; + + if( thetaIdx >= FREAK_NB_ORIENTATION ) + thetaIdx -= FREAK_NB_ORIENTATION; + + // extract descriptor at the computed orientation + for( int i = FREAK_NB_POINTS; i--; ) + { + meanIntensity(image, imgIntegral, keypoints[k].x, keypoints[k].y, + *(kpScaleIdx + k), thetaIdx, &i, patternLookup, pointsValue); + } + + // Extract descriptor + int cnt = 0; + for( int n = 0; n<4; n++ ) + { + for (int m = 8; m >= 1; m--) + { + for(int kk = (n*16)+15; kk >= (n*16); --kk, ++cnt) + { + descriptors[(k * 64) + kk] *= 2; + if (pointsValue[(int)descriptionPairs[cnt].x] >= pointsValue[(int)descriptionPairs[cnt].y]) + descriptors[(k * 64) + kk] += 1; + } + } + } +} \ No newline at end of file diff --git a/kernel/integral_image.cl b/kernel/integral_image.cl new file mode 100644 index 0000000..da53953 --- /dev/null +++ b/kernel/integral_image.cl @@ -0,0 +1,13 @@ +__kernel void integral_image( + __read_only image2d_t int_img, // grayescale image - height X width + __write_only image2d_t out_img // (height+1) X (width+1) with zero values + ) +{ + sampler_t const sampler = CLK_ADDRESS_CLAMP | CLK_FILTER_NEAREST; + int2 coord = (int2)(get_global_id(0)+1, get_global_id(1)+1); + int res = 0; + for (int i = 0; i < coord.x; i++) + for (int j = 0; j < coord.y; j++) + res += read_imageui(int_img, sampler, (int2)(i, j)); // bottom right corner + write_imageui(out_img, coord, res); +} \ No newline at end of file diff --git a/main2.cpp b/main2.cpp new file mode 100644 index 0000000..23bd20e --- /dev/null +++ b/main2.cpp @@ -0,0 +1,42 @@ +// +// Created by root on 8/5/21. +// + +#include +#include "cl_class.h" +#include +#include +#include +#include +// Maximum number of corners. +#define FAST_COUNT (1 << 18) + +int main() { + opencl gpu; + gpu.make_kernel("compute_residual"); + cv::Mat img=cv::Mat(cv::Size(700, 500),CV_8UC1); + gpu.write_buf(0,0,img); + gpu.write_buf(0,1,img); + cl_float3 pose[1]={0.0}; + gpu.write_buf(0,2,1,pose); + gpu.write_buf(0,3,1,pose); + cl_float3 f[300]={0}; + gpu.write_buf(0,4,300,f); + cl_float2 px[300]={0}; + gpu.write_buf(0,5,300,px); + float e[300]={0}; + gpu.write_buf(0,7,300,e); + float H[300*9]={0}; + gpu.write_buf(0,8,9*300,H); + cl_float3 J[300]; + gpu.write_buf(0,9,300,J); + float chi2[300]={0.01}; + gpu.write_buf(0,10,300,chi2); + for(int i=0;i<10;++i){ + vio::SparseImgAlignGpu img_align(4, 2,30, vio::SparseImgAlignGpu::GaussNewton, true,&gpu); + img_align.run(); + } + gpu.clear_buf(); + std::cerr<<"finished\n"; + return 0; +} \ No newline at end of file diff --git a/src/math_utils.cpp b/src/math_utils.cpp new file mode 100644 index 0000000..d09d3ba --- /dev/null +++ b/src/math_utils.cpp @@ -0,0 +1,202 @@ +/* + * math_utils.cpp + * + * Created on: Jul 20, 2012 + * Author: cforster + */ + +#include + +namespace vk { + +using namespace Eigen; + +Vector3d +triangulateFeatureNonLin(const Matrix3d& R, const Vector3d& t, + const Vector3d& feature1, const Vector3d& feature2 ) +{ + Vector3d f2 = R * feature2; + Vector2d b; + b[0] = t.dot(feature1); + b[1] = t.dot(f2); + Matrix2d A; + A(0,0) = feature1.dot(feature1); + A(1,0) = feature1.dot(f2); + A(0,1) = -A(1,0); + A(1,1) = -f2.dot(f2); + Vector2d lambda = A.inverse() * b; + Vector3d xm = lambda[0] * feature1; + Vector3d xn = t + lambda[1] * f2; + return ( xm + xn )/2; +} + +bool +depthFromTriangulationExact( + const Matrix3d& R_r_c, + const Vector3d& t_r_c, + const Vector3d& f_r, + const Vector3d& f_c, + double& depth_in_r, + double& depth_in_c) +{ + // bearing vectors (f_r, f_c) do not need to be unit length + const Vector3d f_c_in_r(R_r_c*f_c); + const double a = f_c_in_r.dot(f_r) / t_r_c.dot(f_r); + const double b = f_c_in_r.dot(t_r_c); + const double denom = (a*b - f_c_in_r.dot(f_c_in_r)); + + if(abs(denom) < 0.000001) + return false; + + depth_in_c = (b-a*t_r_c.dot(t_r_c)) / denom; + depth_in_r = (t_r_c + f_c_in_r*depth_in_c).norm(); + return true; +} + +double +reprojError(const Vector3d& f1, + const Vector3d& f2, + double error_multiplier2) +{ + Vector2d e = project2d(f1) - project2d(f2); + return error_multiplier2 * e.norm(); +} + +double +computeInliers(const vector& features1, // c1 + const vector& features2, // c2 + const Matrix3d& R, // R_c1_c2 + const Vector3d& t, // c1_t + const double reproj_thresh, + double error_multiplier2, + vector& xyz_vec, // in frame c1 + vector& inliers, + vector& outliers) +{ + inliers.clear(); inliers.reserve(features1.size()); + outliers.clear(); outliers.reserve(features1.size()); + xyz_vec.clear(); xyz_vec.reserve(features1.size()); + double tot_error = 0; + //triangulate all features and compute reprojection errors and inliers + for(size_t j=0; j reproj_thresh || e2 > reproj_thresh) + outliers.push_back(j); + else + { + inliers.push_back(j); + tot_error += e1+e2; + } + } + return tot_error; +} + +void +computeInliersOneView(const vector & feature_sphere_vec, + const vector & xyz_vec, + const Matrix3d &R, + const Vector3d &t, + const double reproj_thresh, + const double error_multiplier2, + vector& inliers, + vector& outliers) +{ + inliers.clear(); inliers.reserve(xyz_vec.size()); + outliers.clear(); outliers.reserve(xyz_vec.size()); + for(size_t j = 0; j < xyz_vec.size(); j++ ) + { + double e = reprojError(feature_sphere_vec[j], + R.transpose() * ( xyz_vec[j] - t ), + error_multiplier2); + if(e < reproj_thresh) + inliers.push_back(j); + else + outliers.push_back(j); + } +} + +Vector3d +dcm2rpy(const Matrix3d &R) +{ + Vector3d rpy; + rpy[1] = atan2( -R(2,0), sqrt( pow( R(0,0), 2 ) + pow( R(1,0), 2 ) ) ); + if( fabs( rpy[1] - M_PI/2 ) < 0.00001 ) + { + rpy[2] = 0; + rpy[0] = -atan2( R(0,1), R(1,1) ); + } + else + { + if( fabs( rpy[1] + M_PI/2 ) < 0.00001 ) + { + rpy[2] = 0; + rpy[0] = -atan2( R(0,1), R(1,1) ); + } + else + { + rpy[2] = atan2( R(1,0)/cos(rpy[1]), R(0,0)/cos(rpy[1]) ); + rpy[0] = atan2( R(2,1)/cos(rpy[1]), R(2,2)/cos(rpy[1]) ); + } + } + return rpy; +} + +Matrix3d +rpy2dcm(const Vector3d &rpy) +{ + Matrix3d R1; + R1(0,0) = 1.0; R1(0,1) = 0.0; R1(0,2) = 0.0; + R1(1,0) = 0.0; R1(1,1) = cos(rpy[0]); R1(1,2) = -sin(rpy[0]); + R1(2,0) = 0.0; R1(2,1) = -R1(1,2); R1(2,2) = R1(1,1); + + Matrix3d R2; + R2(0,0) = cos(rpy[1]); R2(0,1) = 0.0; R2(0,2) = sin(rpy[1]); + R2(1,0) = 0.0; R2(1,1) = 1.0; R2(1,2) = 0.0; + R2(2,0) = -R2(0,2); R2(2,1) = 0.0; R2(2,2) = R2(0,0); + + Matrix3d R3; + R3(0,0) = cos(rpy[2]); R3(0,1) = -sin(rpy[2]); R3(0,2) = 0.0; + R3(1,0) = -R3(0,1); R3(1,1) = R3(0,0); R3(1,2) = 0.0; + R3(2,0) = 0.0; R3(2,1) = 0.0; R3(2,2) = 1.0; + + return R3 * R2 * R1; +} + +Quaterniond +angax2quat(const Vector3d& n, const double& angle) +{ + // n must be normalized! + double s(sin(angle/2)); + return Quaterniond( cos(angle/2), n[0]*s, n[1]*s, n[2]*s ); +} + + +Matrix3d +angax2dcm(const Vector3d& n, const double& angle) +{ + // n must be normalized + Matrix3d sqewn(sqew(n)); + return Matrix3d(Matrix3d::Identity() + sqewn*sin(angle) + sqewn*sqewn*(1-cos(angle))); +} + +double +sampsonusError(const Vector2d &v2Dash, const Matrix3d& Essential, const Vector2d& v2) +{ + Vector3d v3Dash = unproject2d(v2Dash); + Vector3d v3 = unproject2d(v2); + + double dError = v3Dash.transpose() * Essential * v3; + + Vector3d fv3 = Essential * v3; + Vector3d fTv3Dash = Essential.transpose() * v3Dash; + + Vector2d fv3Slice = fv3.head<2>(); + Vector2d fTv3DashSlice = fTv3Dash.head<2>(); + + return (dError * dError / (fv3Slice.dot(fv3Slice) + fTv3DashSlice.dot(fTv3DashSlice))); +} + +} // end namespace vk diff --git a/src/robust_cost.cpp b/src/robust_cost.cpp new file mode 100644 index 0000000..62cda4c --- /dev/null +++ b/src/robust_cost.cpp @@ -0,0 +1,166 @@ +/** +* This file is part of dvo. +* +* Copyright 2012 Christian Kerl (Technical University of Munich) +* For more information see . +* +* dvo is free software: you can redistribute it and/or modify +* it under the terms of the GNU General Public License as published by +* the Free Software Foundation, either version 3 of the License, or +* (at your option) any later version. +* +* dvo is distributed in the hope that it will be useful, +* but WITHOUT ANY WARRANTY; without even the implied warranty of +* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the +* GNU General Public License for more details. +* +* You should have received a copy of the GNU General Public License +* along with dvo. If not, see . +*/ + +#include +#include +#include +#include +#include + +namespace vk { +namespace robust_cost { + +const float TDistributionScaleEstimator::INITIAL_SIGMA = 5.0f; +const float TDistributionScaleEstimator::DEFAULT_DOF = 5.0f; + +TDistributionScaleEstimator:: +TDistributionScaleEstimator(const float dof) : + dof_(dof), + initial_sigma_(INITIAL_SIGMA) +{} + +float TDistributionScaleEstimator:: +compute(std::vector& errors) const +{ + std::cerr<<"42\n"; + float initial_lamda = 1.0f / (initial_sigma_ * initial_sigma_); + int num = 0; + float lambda = initial_lamda; + int iterations = 0; + do + { + ++iterations; + initial_lamda = lambda; + num = 0; + lambda = 0.0f; + + for(std::vector::iterator it=errors.begin(); it!=errors.end(); ++it) + { + if(std::isfinite(*it)) + { + ++num; + const float error2 = (*it)*(*it); + lambda += error2 * ( (dof_ + 1.0f) / (dof_ + initial_lamda * error2) ); + } + } + lambda = float(num) / lambda; + } while(std::abs(lambda - initial_lamda) > 1e-3); + + return std::sqrt(1.0f / lambda); +} + +const float MADScaleEstimator::NORMALIZER = 1.48f; // 1 / 0.6745 + +float MADScaleEstimator:: +compute(std::vector& errors) const +{ + std::cerr<<"74\n"; + // error must be in absolute values! + return NORMALIZER * vk::getMedian(errors); +} + +float NormalDistributionScaleEstimator:: +compute(std::vector& errors) const +{ + std::cerr<<"82\n"; + const float mean = std::accumulate(errors.begin(), errors.end(), 0)/errors.size(); + float var = 0.0; + std::for_each(errors.begin(), errors.end(), [&](const float d) { + var += (d - mean) * (d - mean); + }); + return std::sqrt(var); // return standard deviation +} + +const float TukeyWeightFunction::DEFAULT_B = 4.6851f; + +TukeyWeightFunction::TukeyWeightFunction(const float b) +{ + configure(b); +} + +float TukeyWeightFunction::value(const float& x) const +{ + const float x_square = x * x; + if(x_square <= b_square) + { + const float tmp = 1.0f - x_square / b_square; + return tmp * tmp; + } + else + { + return 0.0f; + } +} + +void TukeyWeightFunction:: +configure(const float& param) +{ + b_square = param * param; +} + +const float TDistributionWeightFunction::DEFAULT_DOF = 5.0f; + +TDistributionWeightFunction:: +TDistributionWeightFunction(const float dof) +{ + configure(dof); +} + +float TDistributionWeightFunction:: +value(const float & x) const +{ + return ((dof_ + 1.0f) / (dof_ + (x * x))); +} + +void TDistributionWeightFunction:: +configure(const float& param) +{ + dof_ = param; + normalizer_ = dof_ / (dof_ + 1.0f); +} + +const float HuberWeightFunction::DEFAULT_K = 1.345f; + +HuberWeightFunction:: +HuberWeightFunction(const float k) +{ + configure(k); +} + +void HuberWeightFunction:: +configure(const float& param) +{ + k = param; +} + +float HuberWeightFunction:: +value(const float& t) const +{ + const float t_abs = std::abs(t); + if(t_abs < k) + return 1.0f; + else + return k / t_abs; +} + +} // namespace robust_cost +} // namespace vk + + diff --git a/src/sparse_img_align_gpu.cpp b/src/sparse_img_align_gpu.cpp new file mode 100644 index 0000000..61a9be7 --- /dev/null +++ b/src/sparse_img_align_gpu.cpp @@ -0,0 +1,268 @@ +// This file is part of SVO - Semi-direct Visual Odometry. +// +// Copyright (C) 2014 Christian Forster +// (Robotics and Perception Group, University of Zurich, Switzerland). +// +// SVO is free software: you can redistribute it and/or modify it under the +// terms of the GNU General Public License as published by the Free Software +// Foundation, either version 3 of the License, or any later version. +// +// SVO is distributed in the hope that it will be useful, but WITHOUT ANY +// WARRANTY; without even the implied warranty of MERCHANTABILITY or FITNESS +// FOR A PARTICULAR PURPOSE. See the GNU General Public License for more details. +// +// You should have received a copy of the GNU General Public License +// along with this program. If not, see . + +#include + +namespace vio { + +SparseImgAlignGpu::SparseImgAlignGpu( + int max_level, int min_level, int n_iter, + Method method, bool verbose,opencl* residual) : + max_level_(max_level), + min_level_(min_level), + residual_(residual) +{ + n_iter_ = n_iter; + n_iter_init_ = n_iter_; + method_ = method; + verbose_ = verbose; + eps_ = 1e-10; +// data = fopen((std::string(getenv("HOME"))+"/Project/data.txt").c_str(), "w+"); +} +int SparseImgAlignGpu::feature_counter(std::string filename) { + int counter = 0; + std::string line; + std::ifstream myfile (filename); + if (myfile.is_open()) { + getline(myfile, line); + while(line.find("features") == std::string::npos) + getline(myfile, line); + while (!myfile.eof()) { + getline(myfile, line); + counter ++; + } + myfile.close(); + } + else + std::cout << "Unable to open file"; + return counter; +} +void SparseImgAlignGpu::data_reader(std::string filename, float *rp, float *cp, float *c, float *fts, float *fts_px) { + std::string line; + std::ifstream myfile (filename); + if (myfile.is_open()) { + while (true) { + getline(myfile, line); + if (line.find("ref_frame") != std::string::npos) { + getline(myfile, line); + std::string substring; + std::stringstream ss(line); + getline(ss, substring, ','); + *(rp) = atof(substring.c_str()); + getline(ss, substring, ','); + *(rp + 1) = atof(substring.c_str()); + getline(ss, substring, ','); + *(rp + 2) = atof(substring.c_str()); + } else if (line.find("cur_frame") != std::string::npos) { + getline(myfile, line); + std::string substring; + std::stringstream ss(line); + getline(ss, substring, ','); + *(cp) = atof(substring.c_str()); + getline(ss, substring, ','); + *(cp + 1) = atof(substring.c_str()); + getline(ss, substring, ','); + *(cp + 2) = atof(substring.c_str()); + } else if (line.find("camera") != std::string::npos) { + getline(myfile, line); + std::string substring; + std::stringstream ss(line); + getline(ss, substring, ','); + *(c) = atof(substring.c_str()); + getline(ss, substring, ','); + *(c + 1) = atof(substring.c_str()); + getline(ss, substring, ','); + *(c + 2) = atof(substring.c_str()); + getline(ss, substring, ','); + *(c + 3) = atof(substring.c_str()); + getline(ss, substring, ','); + *(c + 4) = atof(substring.c_str()); + } + else{ + break; + } + } + if (line.find("features") != std::string::npos) { + int fts_i = 0; + int fts_px_i = 0; + while (!myfile.eof()) { + getline(myfile, line); + std::string substring; + std::stringstream ss(line); + getline(ss, substring, ','); + *(fts + (fts_i)) = atof(substring.c_str()); + getline(ss, substring, ','); + *(fts + (fts_i + 1)) = atof(substring.c_str()); + getline(ss, substring, ','); + *(fts + (fts_i + 2)) = atof(substring.c_str()); + fts_i += 3; + getline(ss, substring, ','); + *(fts_px + (fts_px_i)) = atof(substring.c_str()); + getline(ss, substring, ','); + *(fts_px + (fts_px_i + 1)) = atof(substring.c_str()); + fts_px_i += 2; + } + } + myfile.close(); + } + else + std::cout << "Unable to open file"; + return; +} +/*void draw(double* points, int f) { + for (int level = 2; level <= 4; level++) { + double scale = pow(2.0, -level); + cv::Mat ref_img = cv::imread(std::string(TEST_DATA_DIR)+"/fer"+ std::to_string(level)+".png"); + for (int i = 0; i < f; i++) { + cv::Point p; + p.x = points[2*i] * scale; + p.y = points[2*i+1] * scale; + cv::circle(ref_img, p, 1, (0, 0, 255)); + } + cv::imwrite("/root/Projects/p_25_vio_opencl/Vio_OpenCL/test/res/ref"+ std::to_string(level)+".png",ref_img); + } +}*/ +size_t SparseImgAlignGpu::run() +{ + std::string datafilename = std::string(TEST_DATA_DIR)+"/data.txt"; + int fts_size = feature_counter(datafilename); + float* fts = new float[3 * fts_size]; + float* fts_px = new float[2 * fts_size]; + reset(); + float camera[5]; + float ref_pos[3]; + float cur_pos[3]; + data_reader(datafilename, ref_pos, cur_pos, camera, fts, fts_px); + cl_float3 ref_[1],cur_[1]; + ref_[0].x=ref_pos[0]; + ref_[0].y=ref_pos[1]; + ref_[0].z=ref_pos[2]; + cur_[0].x=cur_pos[0]; + cur_[0].y=cur_pos[1]; + cur_[0].z=cur_pos[2]; + residual_->reload_buf(0,2,cur_); + residual_->reload_buf(0,3,ref_); + cl_float3 features[fts_size]; + cl_float2 featue_px[fts_size]; + + for (int i = 0; i < fts_size; ++i) { + features[i].x = fts[3*i]; + features[i].y = fts[3*i+1]; + features[i].z = fts[3*i+2]; + } + for (int i = 0; i < fts_size; ++i) { + featue_px[i].x = fts_px[2*i]; + featue_px[i].y = fts_px[2*i+1]; + } + + feature_counter_ = fts_size; // is used to compute the index of the cached jacobian + level_=4; + residual_->reload_buf(0,4,features); + residual_->reload_buf(0,5,featue_px); + Sophus::SE2 T_cur(cur_pos[2],Eigen::Vector2d(cur_pos[0],cur_pos[1]));///TODO temporary, we can remove it + for(level_ = max_level_; level_ >= min_level_; --level_) + { + cv::Mat cur_img = cv::imread(std::string(TEST_DATA_DIR)+"/cur"+ std::to_string(level_)+".png"); + cv::Mat ref_img = cv::imread(std::string(TEST_DATA_DIR)+"/fer"+ std::to_string(level_)+".png"); + residual_->reload_buf(0,0,cur_img); + residual_->reload_buf(0,1,ref_img); + residual_->write_buf(0,6,level_); + mu_ = 1.0; + optimize(T_cur); + } + cl_float3 pos[1]={0}; + residual_->read(0,2,pos); + std::cerr<<"pos : "<write_buf(0,11,scale_); + residual_->reload_buf(0,10,chi2_); + residual_->reload_buf(0,8,H); + residual_->reload_buf(0,9,J); + /// TODO GPU compute based on the feature + residual_->run(0,feature_counter_); + float error[300]={0.0}; + float chi[300]={0.0}; + residual_->read(0,10,feature_counter_,chi); + residual_->read(0,7,feature_counter_,error); + for(int i = 1; i < feature_counter_; ++i){ + error[0] += error[i]; + chi[0] += chi[i]; + } + scale_ =error[0]; + return chi[0]; +} + +int SparseImgAlignGpu::solve() +{ + float H[300*9]={0.0}; + cl_float3 J[300]={0.0}; + residual_->read(0,8,feature_counter_*9,H); + residual_->read(0,9,feature_counter_,J); + for(int i = 1; i < feature_counter_; ++i){ + H[0] += H[i*9]; + H[1] += H[i*9+1]; + H[2] += H[i*9+2]; + H[3] += H[i*9+3]; + H[4] += H[i*9+4]; + H[5] += H[i*9+5]; + H[6] += H[i*9+6]; + H[7] += H[i*9+7]; + H[8] += H[i*9+8]; + J[0].x -= J[i].x; + J[0].y -= J[i].y; + J[0].z -= J[i].z; + } + double Hd[9]={(double)H[0],(double)H[1],(double)H[2], + (double)H[3],(double)H[4],(double)H[5], + (double)H[6],(double)H[7],(double)H[8]}; + double Jd[3]={(double)J[0].x,(double)J[0].y,(double)J[0].z}; + x_ = Eigen::Matrix(Hd).ldlt().solve(Eigen::Vector3d(Jd[0],Jd[1],Jd[2])); + if((bool) std::isnan((double) x_[0])) + return 0; + return 1; +} +void SparseImgAlignGpu::update() +{ +/// TODO the update situation may have a smarter solution +/// TODO the update situation may have a smarter solution + cl_float3 pos[1]={0.0,0.0,0.0}; + residual_->read(0,2,pos); + Sophus::SE2 update = Sophus::SE2(pos[0].z,Eigen::Vector2d(pos[0].x,pos[0].y)) * Sophus::SE2::exp(-1.0*x_); + pos[0].x=(float)update.translation()(0); + pos[0].y=(float)update.translation()(1); + pos[0].z=(float)atan(update.so2().unit_complex().imag()/update.so2().unit_complex().real()); + residual_->reload_buf(0,2,pos); +} + +void SparseImgAlignGpu::startIteration() +{} + +void SparseImgAlignGpu::finishIteration() +{ +} + +} // namespace vio + diff --git a/test/data/cur2.png b/test/data/cur2.png new file mode 100644 index 0000000..0cbae83 Binary files /dev/null and b/test/data/cur2.png differ diff --git a/test/data/cur3.png b/test/data/cur3.png new file mode 100644 index 0000000..173c5b0 Binary files /dev/null and b/test/data/cur3.png differ diff --git a/test/data/cur4.png b/test/data/cur4.png new file mode 100644 index 0000000..2aadbfb Binary files /dev/null and b/test/data/cur4.png differ diff --git a/test/data/data.txt b/test/data/data.txt new file mode 100644 index 0000000..b7fc29f --- /dev/null +++ b/test/data/data.txt @@ -0,0 +1,85 @@ +ref_frame: + -0.326890,0.009666,1.254757 +cur_frame: +-0.326890,0.009666,1.254757 +camera: +296.724480,395.357280,317.877600,238.752640,1.016870 + features: +-0.649845,-0.351804,0.548849,65.626709,56.799358 +-0.648425,-0.352782,0.551745,66.660110,56.643040 +0.538037,-0.545661,0.900806,473.341705,28.676302 +0.538337,-0.542014,0.901982,473.459686,30.038052 +-0.611747,-0.287029,0.511483,60.513573,77.859436 +0.505807,-0.460219,0.892039,470.488708,53.739651 +-0.391810,-0.176149,0.489420,111.763100,115.286110 +-0.255603,-0.058511,0.362289,124.788757,179.859940 +-0.254965,-0.057964,0.363864,125.720444,180.545792 +-0.481319,-0.155208,0.936546,167.409195,174.103531 +-0.097664,-0.138420,1.040516,287.678375,181.723557 +-0.576794,-0.204837,1.112729,166.636932,167.188980 +-0.347883,-0.176067,1.225530,229.154205,178.922424 +-0.353727,-0.149093,1.185631,224.753128,186.454300 +-0.351608,-0.145035,1.177231,224.635208,187.506256 +-0.356308,-0.136734,1.151653,221.485016,189.465897 +-0.347270,-0.084129,1.254798,230.716705,210.618484 +-0.271202,-0.049082,1.219884,247.042603,221.671478 +0.551335,-0.024829,0.900301,492.609039,228.268005 +0.570153,-0.026209,0.918852,494.399292,227.940765 +0.593849,-0.027990,0.943331,496.370331,227.543304 +0.538455,0.042511,0.896662,489.880554,256.846283 +-0.410921,0.199957,1.465923,230.130569,295.644165 +-0.418205,0.205574,1.478655,229.414886,296.692078 +0.706988,0.340967,1.050721,500.369568,356.020905 +-1.773052,0.703438,2.013429,97.468231,355.264709 +-0.507049,0.394948,1.237993,197.115158,364.083618 +-0.505709,0.397308,1.246433,198.122162,364.112518 +-0.046137,0.358122,1.028613,303.971924,382.569611 +-0.042918,0.358798,1.030556,304.965149,382.584106 +-0.026664,0.361984,1.039650,309.922577,382.648041 +-0.023372,0.362622,1.041471,310.916443,382.657440 +-0.046169,0.360681,1.028121,303.965210,383.568085 +-0.042947,0.361373,1.030094,304.959534,383.582489 +-0.023289,0.400521,1.039641,310.996948,396.418610 +-0.023522,0.397142,1.036895,310.905396,395.598389 +-0.520659,0.499070,1.226407,195.651825,394.854034 +-0.518963,0.501603,1.233899,196.654175,394.868164 +-0.522851,0.510476,1.242953,196.763367,396.306335 +-0.618554,-0.289543,0.509116,58.533249,77.001205 +-0.567769,-0.273975,0.557984,81.440971,86.736328 +-0.570194,-0.275719,0.552642,79.710831,85.304115 +-0.614502,-0.284236,0.507294,58.608025,78.964867 +-0.609955,-0.284518,0.510723,60.550095,78.821106 +-0.631422,-0.320436,0.530292,63.085056,66.468994 +-0.320845,-0.398865,1.298285,242.146652,113.311325 +0.489002,-0.308087,0.873590,474.750366,107.064598 +-0.065107,-0.329243,1.251848,301.440216,127.998520 +-0.070308,-0.263461,1.136162,298.211243,140.561432 +-0.404364,-0.200603,1.105117,206.199829,164.933563 +-0.262542,-0.061586,0.381638,128.352280,179.516068 +-0.279101,-0.068491,0.423297,134.297928,178.727234 +-0.268416,-0.063972,0.398420,131.273529,179.495895 +-0.495678,-0.162953,0.959804,166.863388,172.604675 +-0.411479,-0.175127,1.136268,206.882751,175.810242 +-0.420565,-0.122517,0.921132,181.328644,185.751114 +-0.408601,-0.119380,0.905473,182.686615,186.124893 +-0.522424,-0.158378,1.018227,167.462585,177.995071 +-0.436577,-0.125475,0.961936,181.999451,186.719391 +0.763324,-0.179506,1.278473,488.213959,185.380585 +0.735538,-0.175594,1.222357,489.217255,184.252213 +-0.531641,-0.135054,1.152132,179.896881,192.049866 +-0.153214,-0.064019,1.125556,273.996948,214.322815 +-0.250853,-0.022721,1.188315,250.454727,230.615723 +-0.269293,-0.049531,1.232366,248.207672,221.678802 +0.689335,0.138668,1.360214,467.413635,278.832611 +-0.315660,0.228150,1.827959,262.734253,291.856873 +-1.722023,0.597785,1.949498,95.677032,341.527618 +-2.017433,0.720874,2.317486,97.949333,343.460022 +-0.394809,-0.185239,0.488969,111.179558,109.535812 +0.503526,-0.272907,0.866554,480.762085,121.125191 +-0.386096,-0.238207,1.189135,218.027527,156.671371 +-0.358741,-0.249081,1.196880,225.289307,153.097733 +0.392533,-0.493985,0.969992,431.760529,47.796806 +-0.073172,-0.350268,1.270865,299.731842,123.017418 +-0.802860,-0.809967,1.735968,188.040833,64.225998 +0.489783,-0.513333,0.903382,462.721436,36.482620 +-0.414553,-0.168965,0.471132,97.895798,119.288155 \ No newline at end of file diff --git a/test/data/fer2.png b/test/data/fer2.png new file mode 100644 index 0000000..ec5b894 Binary files /dev/null and b/test/data/fer2.png differ diff --git a/test/data/fer3.png b/test/data/fer3.png new file mode 100644 index 0000000..35e48db Binary files /dev/null and b/test/data/fer3.png differ diff --git a/test/data/fer4.png b/test/data/fer4.png new file mode 100644 index 0000000..a5a047b Binary files /dev/null and b/test/data/fer4.png differ diff --git a/test/data0/test1.png b/test/data0/test1.png new file mode 100644 index 0000000..df6935b Binary files /dev/null and b/test/data0/test1.png differ diff --git a/test/res/ref2.png b/test/res/ref2.png new file mode 100644 index 0000000..d2fca19 Binary files /dev/null and b/test/res/ref2.png differ diff --git a/test/res/ref3.png b/test/res/ref3.png new file mode 100644 index 0000000..d4224fa Binary files /dev/null and b/test/res/ref3.png differ diff --git a/test/res/ref4.png b/test/res/ref4.png new file mode 100644 index 0000000..b15bda9 Binary files /dev/null and b/test/res/ref4.png differ