diff --git a/.gitignore b/.gitignore new file mode 100644 index 0000000..b24d71e --- /dev/null +++ b/.gitignore @@ -0,0 +1,50 @@ +# These are some examples of commonly ignored file patterns. +# You should customize this list as applicable to your project. +# Learn more about .gitignore: +# https://www.atlassian.com/git/tutorials/saving-changes/gitignore + +# Node artifact files +node_modules/ +dist/ + +# Compiled Java class files +*.class + +# Compiled Python bytecode +*.py[cod] + +# Log files +*.log + +# Package files +*.jar + +# Maven +target/ +dist/ + +# JetBrains IDE +.idea/ + +# Unit test reports +TEST*.xml + +# Generated by MacOS +.DS_Store + +# Generated by Windows +Thumbs.db + +# Applications +*.app +*.exe +*.war + +# Large media files +*.mp4 +*.tiff +*.avi +*.flv +*.mov +*.wmv + diff --git a/.idea/misc.xml b/.idea/misc.xml index 339cee9..1dee752 100644 --- a/.idea/misc.xml +++ b/.idea/misc.xml @@ -1,6 +1,6 @@ - + \ No newline at end of file diff --git a/GPU_version/vio/gtsam/imu_integration.cpp b/GPU_version/vio/gtsam/imu_integration.cpp index d2e751b..d8e50b0 100755 --- a/GPU_version/vio/gtsam/imu_integration.cpp +++ b/GPU_version/vio/gtsam/imu_integration.cpp @@ -5,14 +5,14 @@ #include "imu_integration.h" #include #include -#include "gpu_svo/feature.h" -#include "gpu_svo/point.h" -#include "gpu_svo/svoVisionFactor.h" +#include "vio/feature.h" +#include "vio/point.h" +#include "vio/svoVisionFactor.h" #include #include -#include -#include -#include +#include +#include +#include #define debug 0 Imu_Integration::Imu_Integration(Sophus::SE3& SE_init){ graphPtr=std::make_shared(); diff --git a/GPU_version/vio/gtsam/imu_integration.h b/GPU_version/vio/gtsam/imu_integration.h index 11eb514..58dfa92 100644 --- a/GPU_version/vio/gtsam/imu_integration.h +++ b/GPU_version/vio/gtsam/imu_integration.h @@ -13,7 +13,7 @@ #include #include #include -#include +#include using gtsam::symbol_shorthand::B; // Bias (ax,ay,az,gx,gy,gz) using gtsam::symbol_shorthand::V; // Vel (xdot,ydot,zdot) diff --git a/GPU_version/vio/gtsam/svoVisionFactor.h b/GPU_version/vio/gtsam/svoVisionFactor.h index 01ad6da..4011177 100644 --- a/GPU_version/vio/gtsam/svoVisionFactor.h +++ b/GPU_version/vio/gtsam/svoVisionFactor.h @@ -10,11 +10,11 @@ #include #include #include -#include -#include -#include +#include +#include +#include -namespace svo { +namespace vio { class VisionFactor: public gtsam::NoiseModelFactor1 { diff --git a/GPU_version/vio/gtsam/svoVisionFactorLandMark.h b/GPU_version/vio/gtsam/svoVisionFactorLandMark.h index 69ed0c8..5f8f636 100644 --- a/GPU_version/vio/gtsam/svoVisionFactorLandMark.h +++ b/GPU_version/vio/gtsam/svoVisionFactorLandMark.h @@ -12,9 +12,9 @@ #include #include #include -#include +#include -namespace svo { +namespace vio { class VisionFactorLandMark: public gtsam::NoiseModelFactor3 { diff --git a/GPU_version/vio/gtsam/svoVisionFactorSmall.h b/GPU_version/vio/gtsam/svoVisionFactorSmall.h index 738bd95..b119e15 100644 --- a/GPU_version/vio/gtsam/svoVisionFactorSmall.h +++ b/GPU_version/vio/gtsam/svoVisionFactorSmall.h @@ -12,9 +12,9 @@ #include #include #include -#include +#include -namespace svo { +namespace vio { class VisionFactor: public gtsam::NoiseModelFactor1 { diff --git a/GPU_version/vio/include/gpu_svo/abstract_camera.h b/GPU_version/vio/include/vio/abstract_camera.h similarity index 97% rename from GPU_version/vio/include/gpu_svo/abstract_camera.h rename to GPU_version/vio/include/vio/abstract_camera.h index 5d643dc..ccd3c98 100644 --- a/GPU_version/vio/include/gpu_svo/abstract_camera.h +++ b/GPU_version/vio/include/vio/abstract_camera.h @@ -33,6 +33,7 @@ public: /// Project from pixels to world coordiantes. Returns a bearing vector of unit length. virtual Vector3d cam2world(const double& x, const double& y) const = 0; + virtual double* params() const =0; /// Project from pixels to world coordiantes. Returns a bearing vector of unit length. virtual Vector3d diff --git a/GPU_version/vio/include/gpu_svo/aligned_mem.h b/GPU_version/vio/include/vio/aligned_mem.h similarity index 100% rename from GPU_version/vio/include/gpu_svo/aligned_mem.h rename to GPU_version/vio/include/vio/aligned_mem.h diff --git a/GPU_version/vio/include/gpu_svo/atan_camera.h b/GPU_version/vio/include/vio/atan_camera.h similarity index 88% rename from GPU_version/vio/include/gpu_svo/atan_camera.h rename to GPU_version/vio/include/vio/atan_camera.h index 49550b2..943b668 100644 --- a/GPU_version/vio/include/gpu_svo/atan_camera.h +++ b/GPU_version/vio/include/vio/atan_camera.h @@ -14,10 +14,11 @@ #define ATAN_CAMERA_H_ #include +#include #include #include -#include -#include +#include +#include namespace vk { @@ -34,6 +35,7 @@ private: double tans_; //!< distortion model coeff double tans_inv_; //!< distortion model coeff bool distortion_; //!< use distortion model? + double* param_; //! Radial distortion transformation factor: returns ration of distorted / undistorted radius. inline double rtrans_factor(double r) const @@ -84,6 +86,14 @@ public: { return 4*fx_*fy_; } + virtual double* params() const{ + param_[0]=fx_; + param_[1]=fy_; + param_[2]=cx_; + param_[3]=cy_; + param_[4]=s_; + return param_; + } }; } // end namespace vk diff --git a/GPU_version/vio/include/gpu_svo/bundle_adjustment.h b/GPU_version/vio/include/vio/bundle_adjustment.h similarity index 98% rename from GPU_version/vio/include/gpu_svo/bundle_adjustment.h rename to GPU_version/vio/include/vio/bundle_adjustment.h index 9eb8a94..d032cde 100644 --- a/GPU_version/vio/include/gpu_svo/bundle_adjustment.h +++ b/GPU_version/vio/include/vio/bundle_adjustment.h @@ -17,7 +17,7 @@ #ifndef SVO_BUNDLE_ADJUSTMENT_H_ #define SVO_BUNDLE_ADJUSTMENT_H_ -#include +#include namespace g2o { class EdgeProjectXYZ2UV; @@ -26,7 +26,7 @@ class VertexSE3Expmap; class VertexSBAPointXYZ; } -namespace svo { +namespace vio { typedef g2o::EdgeProjectXYZ2UV g2oEdgeSE3; typedef g2o::VertexSE3Expmap g2oFrameSE3; @@ -104,6 +104,6 @@ g2oEdgeSE3* createG2oEdgeSE3( double weight = 1); } // namespace ba -} // namespace svo +} // namespace vio #endif // SVO_BUNDLE_ADJUSTMENT_H_ diff --git a/GPU_version/vio/include/gpu_svo/camera_loader.h b/GPU_version/vio/include/vio/camera_loader.h similarity index 89% rename from GPU_version/vio/include/gpu_svo/camera_loader.h rename to GPU_version/vio/include/vio/camera_loader.h index e9d5a94..612ff64 100644 --- a/GPU_version/vio/include/gpu_svo/camera_loader.h +++ b/GPU_version/vio/include/vio/camera_loader.h @@ -9,9 +9,9 @@ #define VIKIT_CAMERA_LOADER_H_ #include -#include -#include -#include +#include +#include +#include namespace vk { namespace camera_loader { diff --git a/GPU_version/vio/include/gpu_svo/cl.hpp b/GPU_version/vio/include/vio/cl.hpp similarity index 100% rename from GPU_version/vio/include/gpu_svo/cl.hpp rename to GPU_version/vio/include/vio/cl.hpp diff --git a/GPU_version/vio/include/gpu_svo/cl_class.h b/GPU_version/vio/include/vio/cl_class.h similarity index 61% rename from GPU_version/vio/include/gpu_svo/cl_class.h rename to GPU_version/vio/include/vio/cl_class.h index 915305e..eb62562 100644 --- a/GPU_version/vio/include/gpu_svo/cl_class.h +++ b/GPU_version/vio/include/vio/cl_class.h @@ -4,7 +4,7 @@ #ifndef VIO_OPENCL_CL_CLASS_H #define VIO_OPENCL_CL_CLASS_H -#include +#include #include #include #include @@ -12,42 +12,41 @@ #include #include #include +#include class kernel{ public: kernel(cl::Program* program,std::string name){_kernel = new cl::Kernel(*program,name.c_str());}; ~kernel(){ + for(auto&& i:_images)i.first.setDestructorCallback((void (*)(_cl_mem *, void *))notify, NULL); + for(auto&& i:_buffers)i.first.first.setDestructorCallback((void (*)(_cl_mem *, void *))notify, NULL); _buffers.clear(); _images.clear(); }; template int32_t write(size_t id/*buffer ID*/,T* buf,cl::CommandQueue* queue,cl::Context* context,size_t buf_size){ - _buffers.push_back(std::pair,size_t>(std::pair(cl::Buffer(*context, CL_MEM_READ_WRITE, sizeof(T) * buf_size), - buf_size),id)); - cl_int err=queue->enqueueWriteBuffer(_buffers.back().first.first,CL_TRUE, 0, sizeof(T) * buf_size, buf); + _buffers.push_back(std::pair,size_t>(std::pair(cl::Buffer(*context, CL_MEM_READ_WRITE | CL_MEM_USE_HOST_PTR + , sizeof(T) * buf_size,buf), + buf_size),id)); + cl_int err=_kernel->setArg(id,_buffers.back().first.first); if(err!=0){ - std::cerr<<"Error C:Kernel, F:write, L:enqueueWriteBuffer\n"<setArg(id,_buffers.back().first.first); - if(err!=0){ - std::cerr<<"Error C:Kernel, F:write, L3:setArg\n"<(cl::Image2D(*context, CL_MEM_READ_ONLY | CL_MEM_COPY_HOST_PTR, - cl::ImageFormat(CL_R, CL_UNSIGNED_INT8), - buf.size().width, - buf.size().height, - 0, - reinterpret_cast(buf.data)),id)); + _images.push_back(std::pair(cl::Image2D(*context, CL_MEM_READ_ONLY | CL_MEM_USE_HOST_PTR, + cl::ImageFormat(CL_R, CL_UNSIGNED_INT8), + buf.size().width, + buf.size().height, + 0, + reinterpret_cast(buf.data)),id)); cl_int err=_kernel->setArg(id,_images.back().first); if(err!=0){ - std::cerr<<"Error C:Kernel, F:write image, L:setArg\n"< int32_t reload(size_t id,T* buf,cl::CommandQueue* queue){ - try{ - if(id>_buffers.size())return CL_MAP_FAILURE; - for(auto i:_buffers)if(i.second==id) - queue->enqueueWriteBuffer(i.first.first,CL_TRUE, 0, sizeof(T) * i.first.second, buf); - return CL_SUCCESS; - }catch (std::exception& err){ - std::cerr<<"Reload buffer on GPU failed\n"<enqueueMapBuffer(i.first.first,CL_TRUE,CL_MAP_WRITE,0,sizeof(T) * i.first.second); + memcpy(Map_buf,buf,sizeof(T) * i.first.second); + if(queue->enqueueUnmapMemObject(i.first.first,Map_buf)!=CL_SUCCESS)std::cerr<<"Reload buffer on GPU failed"<<'\n'; } + return CL_SUCCESS; }; int32_t reload(size_t id,cv::Mat& buf,cl::Context* context){ try{ - for(auto i:_images)if(i.second==id){ - i.first=cl::Image2D(*context, CL_MEM_READ_ONLY | CL_MEM_COPY_HOST_PTR, + for(auto&& i:_images)if(i.second==id){ + i.first.setDestructorCallback((void (*)(_cl_mem *, void *))notify, NULL); + i.first=cl::Image2D(*context, CL_MEM_READ_ONLY | CL_MEM_USE_HOST_PTR, cl::ImageFormat(CL_R, CL_UNSIGNED_INT8), buf.size().width, buf.size().height, 0, reinterpret_cast(buf.data)); - } - + cl_int err=_kernel->setArg(id,i.first); + if(err!=0){ + std::cerr<<"Error C:Kernel, F:write image, L:setArg ID:\t"<,size_t>> _buffers; std::vector> _images; @@ -111,7 +115,7 @@ public: }; class opencl{ public: - opencl(); + opencl(vk::AbstractCamera* cam); ~opencl(); int32_t make_kernel(std::string name){_kernels.push_back(kernel(program,name));}; template @@ -158,34 +162,41 @@ public: } int32_t run(size_t id1/*kernal ID*/,std::size_t x=1,std::size_t y=1,std::size_t z=1) { cl_int err=0; - if(z>1 && y>1){ - err=queue->enqueueNDRangeKernel(*_kernels.at(id1)._kernel, cl::NullRange/*offset*/, cl::NDRange(x,y,z)/*Global*/, cl::NullRange/*local*/); - }else if(z<2 && y>1){ - err=queue->enqueueNDRangeKernel(*_kernels.at(id1)._kernel, cl::NullRange/*offset*/, cl::NDRange(x,y)/*Global*/, cl::NullRange/*local*/); - }else{ - err=queue->enqueueNDRangeKernel(*_kernels.at(id1)._kernel, cl::NullRange/*offset*/, cl::NDRange(x)/*Global*/, cl::NullRange/*local*/); - }; - queue->finish(); - if(err!=0){ - std::cerr<<"kernel: "<<_kernels.at(id1)._kernel->getInfo(); - std::cerr<<" ,Args: "<<_kernels.at(id1)._kernel->getInfo()<<" ,Run out: "<getInfo();++i) + queue->flush(); + if(z>1 && y>1){ + err=queue->enqueueNDRangeKernel(*_kernels.at(id1)._kernel, cl::NullRange/*offset*/, cl::NDRange(x,y,z)/*Global*/, cl::NullRange/*local*/); + }else if(z<2 && y>1){ + err=queue->enqueueNDRangeKernel(*_kernels.at(id1)._kernel, cl::NullRange/*offset*/, cl::NDRange(x,y)/*Global*/, cl::NullRange/*local*/); + }else{ + err=queue->enqueueNDRangeKernel(*_kernels.at(id1)._kernel, cl::NullRange/*offset*/, cl::NDRange(x)/*Global*/, cl::NullRange/*local*/); + }; + if(err!=0){ + std::cerr<<"kernel: "<<_kernels.at(id1)._kernel->getInfo(); + std::cerr<<" ,Args: "<<_kernels.at(id1)._kernel->getInfo()<<" ,Run out: "<getInfo();++i) std::cerr<<"argument type "<getArgInfo(i)<<'\n'; - } - return err; + } + return queue->finish(); } template void read(size_t id1/*kernal ID*/,size_t id2/*buffer ID*/, T* out){ queue->enqueueReadBuffer(_kernels.at(id1).read(id2).first, CL_TRUE, 0, sizeof(T) * _kernels.at(id1).read(id2).second, out); } + template + void read(size_t id1/*kernal ID*/,size_t id2/*buffer ID*/,size_t size/*size*/, T* out){ + T* Map_buf=(T*)queue->enqueueMapBuffer(_kernels.at(id1).read(id2).first,CL_TRUE,CL_MAP_READ,0,sizeof(T) * size); + memcpy(out,Map_buf,sizeof(T) * size); + if(queue->enqueueUnmapMemObject(_kernels.at(id1).read(id2).first,Map_buf)!=CL_SUCCESS)std::cerr<<"Read buffer on GPU failed"<<'\n'; + } void clear_buf(); private: std::vector _kernels; - cl::Context* context = NULL; - cl::Device device; - cl::Program* program = NULL; - cl::CommandQueue* queue=NULL; + cl::Context* context = nullptr; + cl::Device* device = nullptr; + cl::Program* program = nullptr; + cl::CommandQueue* queue= nullptr; + vk::AbstractCamera* cam= nullptr; }; #endif //VIO_OPENCL_CL_CLASS_H diff --git a/GPU_version/vio/include/gpu_svo/config.h b/GPU_version/vio/include/vio/config.h similarity index 99% rename from GPU_version/vio/include/gpu_svo/config.h rename to GPU_version/vio/include/vio/config.h index f6d710d..8ecb89f 100644 --- a/GPU_version/vio/include/gpu_svo/config.h +++ b/GPU_version/vio/include/vio/config.h @@ -21,7 +21,7 @@ #include #include -namespace svo { +namespace vio { using std::string; @@ -177,6 +177,6 @@ private: double nrp,ebp,gbc,abc,iuc,ACC_noise,GYO_noise,svo_ekf,acc_x_off,acc_y_off,gyr_z_off,cmd_cov; }; -} // namespace svo +} // namespace vio #endif // SVO_CONFIG_H_ diff --git a/GPU_version/vio/include/gpu_svo/depth_filter.h b/GPU_version/vio/include/vio/depth_filter.h similarity index 98% rename from GPU_version/vio/include/gpu_svo/depth_filter.h rename to GPU_version/vio/include/vio/depth_filter.h index 9e82df4..04190a9 100644 --- a/GPU_version/vio/include/gpu_svo/depth_filter.h +++ b/GPU_version/vio/include/vio/depth_filter.h @@ -21,11 +21,11 @@ #include #include //#include -#include -#include -#include +#include +#include +#include -namespace svo { +namespace vio { class Frame; class Feature; @@ -165,6 +165,6 @@ protected: void updateSeedsLoop(); }; -} // namespace svo +} // namespace vio #endif // SVO_DEPTH_FILTER_H_ diff --git a/GPU_version/vio/include/gpu_svo/feature.h b/GPU_version/vio/include/vio/feature.h similarity index 97% rename from GPU_version/vio/include/gpu_svo/feature.h rename to GPU_version/vio/include/vio/feature.h index 2184c4b..54b9149 100644 --- a/GPU_version/vio/include/gpu_svo/feature.h +++ b/GPU_version/vio/include/vio/feature.h @@ -17,9 +17,9 @@ #ifndef SVO_FEATURE_H_ #define SVO_FEATURE_H_ -#include +#include -namespace svo { +namespace vio { /// A salient image region that is tracked across frames. struct Feature @@ -70,6 +70,6 @@ struct Feature {} }; -} // namespace svo +} // namespace vio #endif // SVO_FEATURE_H_ diff --git a/GPU_version/vio/include/gpu_svo/feature_alignment.h b/GPU_version/vio/include/vio/feature_alignment.h similarity index 96% rename from GPU_version/vio/include/gpu_svo/feature_alignment.h rename to GPU_version/vio/include/vio/feature_alignment.h index e3b40a8..5befb9e 100644 --- a/GPU_version/vio/include/gpu_svo/feature_alignment.h +++ b/GPU_version/vio/include/vio/feature_alignment.h @@ -17,9 +17,9 @@ #ifndef SVO_FEATURE_ALIGNMENT_H_ #define SVO_FEATURE_ALIGNMENT_H_ -#include +#include -namespace svo { +namespace vio { /// Subpixel refinement of a reference feature patch with the current image. /// Implements the inverse-compositional approach (see "Lucas-Kanade 20 Years on" @@ -58,6 +58,6 @@ bool align2D_NEON( Vector2d& cur_px_estimate); } // namespace feature_alignment -} // namespace svo +} // namespace vio #endif // SVO_FEATURE_ALIGNMENT_H_ diff --git a/GPU_version/vio/include/gpu_svo/feature_detection.h b/GPU_version/vio/include/vio/feature_detection.h similarity index 96% rename from GPU_version/vio/include/gpu_svo/feature_detection.h rename to GPU_version/vio/include/vio/feature_detection.h index 4dc5ec8..7cdf4da 100644 --- a/GPU_version/vio/include/gpu_svo/feature_detection.h +++ b/GPU_version/vio/include/vio/feature_detection.h @@ -17,11 +17,11 @@ #ifndef SVO_FEATURE_DETECTION_H_ #define SVO_FEATURE_DETECTION_H_ -#include -#include -#include +#include +#include +#include -namespace svo { +namespace vio { /// Implementation of various feature detectors. namespace feature_detection { @@ -106,6 +106,6 @@ public: }; } // namespace feature_detection -} // namespace svo +} // namespace vio #endif // SVO_FEATURE_DETECTION_H_ diff --git a/GPU_version/vio/include/gpu_svo/for_it.hpp b/GPU_version/vio/include/vio/for_it.hpp similarity index 95% rename from GPU_version/vio/include/gpu_svo/for_it.hpp rename to GPU_version/vio/include/vio/for_it.hpp index 808f3cf..f5fce79 100755 --- a/GPU_version/vio/include/gpu_svo/for_it.hpp +++ b/GPU_version/vio/include/vio/for_it.hpp @@ -2,8 +2,8 @@ // Created by root on 5/12/21. // -#ifndef GPU_SVO_FOR_IT_H -#define GPU_SVO_FOR_IT_H +#ifndef VIO_FOR_IT_H +#define VIO_FOR_IT_H #include #include #include @@ -84,4 +84,4 @@ for_impl _for(T & t) { return for_impl(t); } -#endif //GPU_SVO_FOR_IT_H +#endif //VIO_FOR_IT_H diff --git a/GPU_version/vio/include/gpu_svo/frame.h b/GPU_version/vio/include/vio/frame.h similarity index 98% rename from GPU_version/vio/include/gpu_svo/frame.h rename to GPU_version/vio/include/vio/frame.h index 3d1fd72..7408805 100644 --- a/GPU_version/vio/include/gpu_svo/frame.h +++ b/GPU_version/vio/include/vio/frame.h @@ -18,17 +18,17 @@ #define SVO_FRAME_H_ #include -#include -#include +#include +#include #include -#include +#include namespace g2o { class VertexSE3Expmap; } typedef g2o::VertexSE3Expmap g2oFrameSE3; -namespace svo { +namespace vio { class Point; struct Feature; @@ -180,6 +180,6 @@ void createImgPyramid(const cv::Mat& img_level_0, int n_levels, ImgPyr& pyr); bool getSceneDepth(const Frame& frame, double& depth_mean, double& depth_min); } // namespace frame_utils -} // namespace svo +} // namespace vio #endif // SVO_FRAME_H_ diff --git a/GPU_version/vio/include/gpu_svo/frame_handler_base.h b/GPU_version/vio/include/vio/frame_handler_base.h similarity index 98% rename from GPU_version/vio/include/gpu_svo/frame_handler_base.h rename to GPU_version/vio/include/vio/frame_handler_base.h index 99b4195..994f048 100644 --- a/GPU_version/vio/include/gpu_svo/frame_handler_base.h +++ b/GPU_version/vio/include/vio/frame_handler_base.h @@ -21,8 +21,8 @@ #include #include #include -#include -#include +#include +#include namespace vk { @@ -30,7 +30,7 @@ class AbstractCamera; //class PerformanceMonitor; } -namespace svo +namespace vio { class Point; class Matcher; diff --git a/GPU_version/vio/include/gpu_svo/frame_handler_mono.h b/GPU_version/vio/include/vio/frame_handler_mono.h similarity index 94% rename from GPU_version/vio/include/gpu_svo/frame_handler_mono.h rename to GPU_version/vio/include/vio/frame_handler_mono.h index 2e3b6ed..96b839f 100644 --- a/GPU_version/vio/include/gpu_svo/frame_handler_mono.h +++ b/GPU_version/vio/include/vio/frame_handler_mono.h @@ -18,14 +18,15 @@ #define SVO_FRAME_HANDLER_H_ #include -#include -#include -#include -#include -#include -#include +#include +#include +#include +#include +#include +#include +#include -namespace svo { +namespace vio { /// Monocular Visual Odometry Pipeline as described in the SVO paper. class FrameHandlerMono : public FrameHandlerBase @@ -78,6 +79,7 @@ protected: opencl* gpu_fast_; ros::Time time_; + /// Initialize the visual odometry algorithm. virtual void initialize(); @@ -104,6 +106,6 @@ protected: void setCoreKfs(size_t n_closest); }; -} // namespace svo +} // namespace vio #endif // SVO_FRAME_HANDLER_H_ diff --git a/GPU_version/vio/include/gpu_svo/global.h b/GPU_version/vio/include/vio/global.h similarity index 96% rename from GPU_version/vio/include/gpu_svo/global.h rename to GPU_version/vio/include/vio/global.h index 60d0f55..69fae99 100644 --- a/GPU_version/vio/include/gpu_svo/global.h +++ b/GPU_version/vio/include/vio/global.h @@ -43,7 +43,7 @@ EIGEN_DEFINE_STL_VECTOR_SPECIALIZATION(Eigen::Vector2d) #define SVO_ERROR_STREAM(x) ROS_ERROR_STREAM(x) -namespace svo +namespace vio { using namespace Eigen; using namespace Sophus; @@ -90,7 +90,7 @@ namespace svo return T2_; } SE2 inverse() const{ - double yaw=atan(T2_.rotation_matrix()(0,1)/T2_.rotation_matrix()(0,0)); + double yaw=atan(T2_.so2().unit_complex().imag()/T2_.so2().unit_complex().real()); if(yaw<0.0) yaw=M_PI-yaw; else @@ -115,6 +115,6 @@ namespace svo class Frame; typedef boost::shared_ptr FramePtr; -} // namespace svo +} // namespace vio #endif // SVO_GLOBAL_H_ diff --git a/GPU_version/vio/include/gpu_svo/homography.h b/GPU_version/vio/include/vio/homography.h similarity index 98% rename from GPU_version/vio/include/gpu_svo/homography.h rename to GPU_version/vio/include/vio/homography.h index 7f5a7b2..527c974 100644 --- a/GPU_version/vio/include/gpu_svo/homography.h +++ b/GPU_version/vio/include/vio/homography.h @@ -19,7 +19,7 @@ #include #include #include -#include +#include namespace vk { diff --git a/GPU_version/vio/include/gpu_svo/initialization.h b/GPU_version/vio/include/vio/initialization.h similarity index 95% rename from GPU_version/vio/include/gpu_svo/initialization.h rename to GPU_version/vio/include/vio/initialization.h index 28fddbf..f9e551b 100644 --- a/GPU_version/vio/include/gpu_svo/initialization.h +++ b/GPU_version/vio/include/vio/initialization.h @@ -17,11 +17,11 @@ #ifndef SVO_INITIALIZATION_H #define SVO_INITIALIZATION_H -#include -#include +#include +#include //#include "../../gtsam/imu_integration.h" -namespace svo { +namespace vio { class FrameHandlerMono; @@ -32,7 +32,7 @@ enum InitResult { FAILURE, NO_KEYFRAME, SUCCESS }; /// Tracks features using Lucas-Kanade tracker and then estimates a homography. class KltHomographyInit { - friend class svo::FrameHandlerMono; + friend class vio::FrameHandlerMono; public: EIGEN_MAKE_ALIGNED_OPERATOR_NEW @@ -82,6 +82,6 @@ void computeHomography( SE2_5& T_cur_from_ref); } // namespace initialization -} // namespace svo +} // namespace vio #endif // SVO_INITIALIZATION_H diff --git a/GPU_version/vio/include/gpu_svo/map.h b/GPU_version/vio/include/vio/map.h similarity index 98% rename from GPU_version/vio/include/gpu_svo/map.h rename to GPU_version/vio/include/vio/map.h index d4a02e0..9f218cb 100644 --- a/GPU_version/vio/include/gpu_svo/map.h +++ b/GPU_version/vio/include/vio/map.h @@ -20,9 +20,9 @@ #include #include #include -#include +#include -namespace svo { +namespace vio { class Point; class Feature; @@ -132,6 +132,6 @@ void frameValidation(Frame* frame, int id); void pointValidation(Point* point, int id); } // namespace map_debug -} // namespace svo +} // namespace vio #endif // SVO_MAP_H_ diff --git a/GPU_version/vio/include/gpu_svo/matcher.h b/GPU_version/vio/include/vio/matcher.h similarity index 98% rename from GPU_version/vio/include/gpu_svo/matcher.h rename to GPU_version/vio/include/vio/matcher.h index 1d37b57..c7c1c6e 100644 --- a/GPU_version/vio/include/gpu_svo/matcher.h +++ b/GPU_version/vio/include/vio/matcher.h @@ -17,7 +17,7 @@ #ifndef SVO_MATCHER_H_ #define SVO_MATCHER_H_ -#include +#include namespace vk { class AbstractCamera; @@ -26,7 +26,7 @@ namespace vk { } } -namespace svo { +namespace vio { class Point; class Frame; @@ -125,6 +125,6 @@ public: void createPatchFromPatchWithBorder(); }; -} // namespace svo +} // namespace vio #endif // SVO_MATCHER_H_ diff --git a/GPU_version/vio/include/gpu_svo/math_utils.h b/GPU_version/vio/include/vio/math_utils.h similarity index 100% rename from GPU_version/vio/include/gpu_svo/math_utils.h rename to GPU_version/vio/include/vio/math_utils.h diff --git a/GPU_version/vio/include/gpu_svo/nlls_solver.h b/GPU_version/vio/include/vio/nlls_solver.h similarity index 93% rename from GPU_version/vio/include/gpu_svo/nlls_solver.h rename to GPU_version/vio/include/vio/nlls_solver.h index cb6320b..58ec2f1 100644 --- a/GPU_version/vio/include/gpu_svo/nlls_solver.h +++ b/GPU_version/vio/include/vio/nlls_solver.h @@ -16,8 +16,8 @@ #include #include #include -#include -#include +#include +#include namespace vk { @@ -63,6 +63,9 @@ protected: 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 @@ -72,6 +75,8 @@ protected: virtual void update (const ModelType& old_model, ModelType& new_model) = 0; + virtual void + update () = 0; virtual void applyPrior (const ModelType& current_model) { } @@ -105,7 +110,7 @@ public: // robust least squares bool use_weights_; - float scale_; + double scale_=2.0; robust_cost::ScaleEstimatorPtr scale_estimator_; robust_cost::WeightFunctionPtr weight_function_; @@ -126,7 +131,7 @@ public: eps_(0.0000000001), iter_(0), use_weights_(false), - scale_(0.0), + scale_(2.0), scale_estimator_(NULL), weight_function_(NULL) { } @@ -137,7 +142,7 @@ public: void optimize(ModelType& model); /// Gauss Newton optimization strategy - void optimizeGaussNewton(ModelType& model); + void optimizeGaussNewton(); /// Levenberg Marquardt optimization strategy void optimizeLevenbergMarquardt(ModelType& model); @@ -164,6 +169,6 @@ public: } // end namespace vk -#include "gpu_svo/nlls_solver_impl.hpp" +#include "vio/nlls_solver_impl.hpp" #endif /* LM_SOLVER_H_ */ diff --git a/GPU_version/vio/include/gpu_svo/nlls_solver_impl.hpp b/GPU_version/vio/include/vio/nlls_solver_impl.hpp similarity index 89% rename from GPU_version/vio/include/gpu_svo/nlls_solver_impl.hpp rename to GPU_version/vio/include/vio/nlls_solver_impl.hpp index 2ce9302..ce1a4f3 100644 --- a/GPU_version/vio/include/gpu_svo/nlls_solver_impl.hpp +++ b/GPU_version/vio/include/vio/nlls_solver_impl.hpp @@ -16,48 +16,30 @@ template void vk::NLLSSolver::optimize(ModelType& model) { if(method_ == GaussNewton) - optimizeGaussNewton(model); + optimizeGaussNewton(); else if(method_ == LevenbergMarquardt) optimizeLevenbergMarquardt(model); } template -void vk::NLLSSolver::optimizeGaussNewton(ModelType& model) +void vk::NLLSSolver::optimizeGaussNewton() { // Compute weight scale if(use_weights_) - computeResiduals(model, false, true); - - // Save the old model to rollback in case of unsuccessful update - ModelType old_model(model); - + computeResiduals(false, true); // perform iterative estimation for (iter_ = 0; iter_ class Base{ public: @@ -28,9 +28,9 @@ public: state_h_(1)=state_(1)+dz*cos(state_(2))-dx*sin(state_(2)); state_h_(0)=state_(0)+dx*cos(state_(2))+dz*sin(state_(2)); Eigen::Matrix R; - R< G; G<<1.0,0,-dx*sin(state_(2))+dz*cos(state_(2)), 0,1.0,-dz*sin(state_(2))-dx*cos(state_(2)), @@ -48,9 +48,9 @@ public: H<<1.0,0,0.5*pow(dt,2)*(ddz*cos(state_(2))-ddx*sin(state_(2))), 0,1.0,-0.5*pow(dt,2)*(ddx*cos(state_(2))+ddz*sin(state_(2))), 0,0,1.0; - Q< R; - R< G; G<<1.0,0,0.5*pow(dt,2)*(ddz*cos(state_(2))-ddx*sin(state_(2))), 0,1.0,-0.5*pow(dt,2)*(ddx*cos(state_(2))+ddz*sin(state_(2))), @@ -82,9 +82,9 @@ public: H<<1.0,0,0, 0,1.0,0, 0,0,1.0; - Q<predict(x,z,pitch,1e-9*time.toNSec()); lock=false; }; - std::pair,svo::SE2_5> UpdateSvo(double x,double z,double pitch,size_t match,ros::Time& time) { + std::pair,vio::SE2_5> UpdateSvo(double x,double z,double pitch,size_t match,ros::Time& time) { while(lock)usleep(5); lock=true; if(pitch<0.0) @@ -138,18 +138,18 @@ public: pitch_f=M_PI-filter_->state_(2); else pitch_f=filter_->state_(2)-M_PI; - return std::pair,svo::SE2_5>(filter_->cov_,svo::SE2_5(-filter_->state_(0),-filter_->state_(1),pitch_f)); + return std::pair,vio::SE2_5>(filter_->cov_,vio::SE2_5(-filter_->state_(0),-filter_->state_(1),pitch_f)); }; - std::pair,svo::SE2_5> get_location(){ + std::pair,vio::SE2_5> get_location(){ double pitch_f; if(filter_->state_(0)<0.0) pitch_f=M_PI-filter_->state_(2); else pitch_f=filter_->state_(2)-M_PI; - return std::pair,svo::SE2_5>(filter_->cov_,svo::SE2_5(-filter_->state_(0),-filter_->state_(1),pitch_f)); + return std::pair,vio::SE2_5>(filter_->cov_,vio::SE2_5(-filter_->state_(0),-filter_->state_(1),pitch_f)); } private: Base* filter_= nullptr; bool lock=false; }; -#endif //GPU_SVO_UKF_H +#endif //VIO_UKF_H diff --git a/GPU_version/vio/include/gpu_svo/vision.h b/GPU_version/vio/include/vio/vision.h similarity index 98% rename from GPU_version/vio/include/gpu_svo/vision.h rename to GPU_version/vio/include/vio/vision.h index 3b95183..1d0bea2 100644 --- a/GPU_version/vio/include/gpu_svo/vision.h +++ b/GPU_version/vio/include/vio/vision.h @@ -8,7 +8,7 @@ #ifndef VIKIT_VISION_H_ #define VIKIT_VISION_H_ -#include +#include #include #include diff --git a/GPU_version/vio/include/gpu_svo/visualizer.h b/GPU_version/vio/include/vio/visualizer.h similarity index 94% rename from GPU_version/vio/include/gpu_svo/visualizer.h rename to GPU_version/vio/include/vio/visualizer.h index 307c27e..2fbf4d3 100644 --- a/GPU_version/vio/include/gpu_svo/visualizer.h +++ b/GPU_version/vio/include/vio/visualizer.h @@ -19,16 +19,16 @@ #include #include -#include +#include #include #include #include #include #include #include -#include +#include -namespace svo { +namespace vio { class Frame; class Point; @@ -57,6 +57,6 @@ public: }; -} // end namespace svo +} // end namespace vio #endif /* SVO_VISUALIZER_H_ */ diff --git a/GPU_version/vio/kernel/compute-residual.cl b/GPU_version/vio/kernel/compute-residual.cl new file mode 100644 index 0000000..baf62f5 --- /dev/null +++ b/GPU_version/vio/kernel/compute-residual.cl @@ -0,0 +1,153 @@ +// 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 + +double2 world2cam(double3 feature) +{ + double r = sqrt(pow(feature.x/feature.z, 2) + pow(feature.y/feature.z, 2)); + double factor = 1.0; + if(S != 0 || r > 0.001) + factor = (atan(r * 2.0 * tan(0.5 * S)) / (r * S)); + return (double2)(C_X + F_X * factor * feature.x/feature.z, C_Y + F_Y * factor * feature.y/feature.z); +} + +double3 xyz_cur(double3 cur, double3 ref, double3 ref_feature) +{ + if(ref.z < 0.0) + ref.z = 3.141592653589793238462643383279502884197169399375 - ref.z; + else + ref.z = ref.z - 3.1415926535897932384626433832795028841971693993; + double cc_ss = cos(cur.z) * cos(ref.z) - sin(cur.z) * sin(ref.z); + double sc_cs = cos(cur.z) * sin(ref.z) + cos(ref.z) * sin(cur.z); + return (double3)(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(double3 xyz_in_f, double* 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 compute_hessain(double3 j, __global double* H, double 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 double3 * cur_pose,//[reference frame pose{x,z,pitch}] + __global double3 * ref_pose,//[current frame pose{x,z,pitch}] + __global double3 * 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 double2 * featue_px, + int level, + __global float * errors, + __global double * Hessian, + __global double3 * Jacobian, + __global double * chi, + double scale_ +) +{ + double 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 + double2 uv_ref = featue_px[f] * scale; + double2 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 + double frame_jac[6]={0.0}; // 2X3 + jacobian_xyz2uv(sqrt(pow(ref_feature[f] - ref_pose[0], 2.0)), &frame_jac); + + // compute bilateral interpolation weights for reference image + double2 subpix= uv_ref - uv_ref_i; + double w_ref_tl = (1.0 - subpix.x) * (1.0 - subpix.y); + double w_ref_tr = subpix.x * (1.0 - subpix.y); + double w_ref_bl = (1.0 - subpix.x) * subpix.y; + double w_ref_br = subpix.x * subpix.y; + + // compute bilateral interpolation weights for the current image + double2 uv_cur_pyr = world2cam(xyz_cur(cur_pose[0], ref_pose[0] ,ref_feature[f])) * scale; + double2 uv_cur_i = floor(uv_cur_pyr); + double2 subpix_uv_cur = uv_cur_pyr - uv_cur_i; + double w_cur_tl = (1.0 - subpix_uv_cur.x) * (1.0 - subpix_uv_cur.y); + double w_cur_tr = subpix_uv_cur.x * (1.0 - subpix_uv_cur.y); + double w_cur_bl = (1.0 - subpix_uv_cur.x) * subpix_uv_cur.y; + double w_cur_br = subpix_uv_cur.x * subpix_uv_cur.y; + float e = 0.0; + double 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]); + double 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; + + double 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)); + + double 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]); + + double 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 + double weight = res/scale_; //1.48f * vk::getMedian(errors) + chi_ += pow(res,2) * weight; + double3 j_row0 = (double3)((dx * frame_jac[0]), (dx * frame_jac[1]), (dx * frame_jac[2])); + double3 j_row1 = (double3)((dy * frame_jac[3]), (dy * frame_jac[4]), (dy * frame_jac[5])); + double3 J = (j_row0 + j_row1) * (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/GPU_version/vio/kernel/fast-gray.cl b/GPU_version/vio/kernel/fast-gray.cl index ce6a927..7622206 100644 --- a/GPU_version/vio/kernel/fast-gray.cl +++ b/GPU_version/vio/kernel/fast-gray.cl @@ -17,7 +17,7 @@ __kernel void fast_gray( // Use global work item as 2D image coordinates. int const x = get_global_id(0); int const y = get_global_id(1); - if(x>16 && x16){ + if(x>5 && x5){ int2 const xy = (int2)(x, y); // Read the candidate pixel. int const p00 = read_imageui(image, sampler, xy).x; diff --git a/GPU_version/vio/launch/Px30.launch b/GPU_version/vio/launch/Px30.launch index 36114e8..b66a344 100644 --- a/GPU_version/vio/launch/Px30.launch +++ b/GPU_version/vio/launch/Px30.launch @@ -1,6 +1,6 @@ - + diff --git a/GPU_version/vio/out.txt b/GPU_version/vio/out.txt index b027790..ab3b540 100644 Binary files a/GPU_version/vio/out.txt and b/GPU_version/vio/out.txt differ diff --git a/GPU_version/vio/param/vo_fast.yaml b/GPU_version/vio/param/vo_fast.yaml index 1ecb630..b4e3359 100644 --- a/GPU_version/vio/param/vo_fast.yaml +++ b/GPU_version/vio/param/vo_fast.yaml @@ -3,7 +3,7 @@ svo: max_n_kfs: 20 #Limit the number of keyframes in the map. This makes nslam essentially. a Visual Odometry. Set to 0 if unlimited number of keyframes are allowed. Minimum number of keyframes is 3. loba_num_iter: 0 #Number of iterations in the local bundle adjustment. quality_min_fts: 10 #If the number of tracked features drops below this threshold. Tracking quality is bad. - quality_max_drop_fts: 60 #If within one frame, this amount of features are dropped. Tracking quality is bad. + quality_max_drop_fts: 90 #If within one frame, this amount of features are dropped. Tracking quality is bad. init_min_tracked: 100 init_min_disparity: 30 structureoptim_max_pts: 50 diff --git a/GPU_version/vio/src/atan_camera.cpp b/GPU_version/vio/src/atan_camera.cpp index c86f027..7be69e0 100644 --- a/GPU_version/vio/src/atan_camera.cpp +++ b/GPU_version/vio/src/atan_camera.cpp @@ -7,8 +7,8 @@ #include -#include -#include +#include +#include namespace vk { @@ -36,11 +36,14 @@ ATANCamera(double width, double height, tans_ = 0.0; distortion_ = false; } + param_ = (double *) malloc(5 * sizeof(double)); } ATANCamera:: ~ATANCamera() -{} +{ + delete param_; +} Vector3d ATANCamera:: cam2world(const double& x, const double& y) const diff --git a/GPU_version/vio/src/cl_class.cpp b/GPU_version/vio/src/cl_class.cpp index a26d455..46cb4bd 100644 --- a/GPU_version/vio/src/cl_class.cpp +++ b/GPU_version/vio/src/cl_class.cpp @@ -1,8 +1,8 @@ // // Created by root on 4/27/21. // -#include "gpu_svo/cl_class.h" -opencl::opencl() { +#include +opencl::opencl(vk::AbstractCamera* cam):cam(cam) { std::vector all_platforms; cl::Platform::get(&all_platforms); if (all_platforms.size() == 0){ @@ -20,29 +20,38 @@ opencl::opencl() { std::cout << " No devices found. Check OpenCL installation!\n"; exit(1); } - device = all_devices[0]; - std::cout << "CL_DEVICE_NAME: " << device.getInfo() <<'\n' - << "CL_DEVICE_OPENCL_C_VERSION: " <()<<'\n' - << "CL_DEVICE_BUILT_IN_KERNELS: " <()<<'\n' - << "CL_DEVICE_COMPILER_AVAILABLE: " <()<<'\n' - << "CL_DEVICE_LOCAL_MEM_SIZE: " <()<<'\n' - << "CL_DEVICE_GLOBAL_MEM_SIZE: " <()<<'\n'; - - context=new cl::Context({ device }); + device = new cl::Device(all_devices[0]); + std::cout << "CL_DEVICE_NAME: " << device->getInfo() <<'\n' + << "CL_DEVICE_OPENCL_C_VERSION: " <getInfo()<<'\n' + << "CL_DEVICE_BUILT_IN_KERNELS: " <getInfo()<<'\n' + << "CL_DEVICE_COMPILER_AVAILABLE: " <getInfo()<<'\n' + << "CL_DEVICE_LOCAL_MEM_SIZE: " <getInfo()<<'\n' + << "CL_DEVICE_GLOBAL_MEM_SIZE: " <getInfo()<<'\n'; + context=new cl::Context({ *device }); cl::Program::Sources sources; read_cl fast(std::string(KERNEL_DIR)+"/fast-gray.cl"); sources.push_back({ fast.src_str, fast.size }); + read_cl compute_residual(std::string(KERNEL_DIR)+"/compute-residual.cl"); + sources.push_back({ compute_residual.src_str, compute_residual.size }); program=new cl::Program(*context, sources); - if(program->build({ device },"-DFAST_THRESH=40") !=0) - std::cout << " Error building: " << program->getBuildInfo(device)<<'\n' - <getBuildInfo(device) << '\n'; - queue=new cl::CommandQueue(*context,device,0,NULL); + double* camera=cam->params(); + std::string options="-DFAST_THRESH=40 -DPATCH_SIZE=2 -DPATCH_HALFSIZE=1 -DF_X="+ std::to_string(camera[0]) + + " -DF_Y="+std::to_string(camera[1])+ + " -DC_X="+std::to_string(camera[2])+ + " -DC_Y="+std::to_string(camera[3])+ + " -DS="+std::to_string(camera[4]); + if(program->build({ *device },options.c_str()) !=0) + std::cout << " Error building: " << program->getBuildInfo(*device)<<'\n' + <getBuildInfo(*device) << '\n'; + queue=new cl::CommandQueue(*context,*device,CL_QUEUE_OUT_OF_ORDER_EXEC_MODE_ENABLE,NULL); } opencl::~opencl() { + delete context; + delete device; } void opencl::clear_buf() { queue->finish(); - queue->flush(); _kernels.clear(); + delete program; } diff --git a/GPU_version/vio/src/config.cpp b/GPU_version/vio/src/config.cpp index 5c3cab7..fe328c5 100644 --- a/GPU_version/vio/src/config.cpp +++ b/GPU_version/vio/src/config.cpp @@ -14,10 +14,10 @@ // You should have received a copy of the GNU General Public License // along with this program. If not, see . -#include -#include +#include +#include -namespace svo { +namespace vio { Config::Config() : trace_name(vk::getParam("svo/trace_name", "svo")), @@ -69,5 +69,5 @@ Config& Config::getInstance() return instance; } -} // namespace svo +} // namespace vio diff --git a/GPU_version/vio/src/depth_filter.cpp b/GPU_version/vio/src/depth_filter.cpp index c7ebf2c..45abf92 100644 --- a/GPU_version/vio/src/depth_filter.cpp +++ b/GPU_version/vio/src/depth_filter.cpp @@ -15,21 +15,21 @@ // along with this program. If not, see . #include -#include -#include -#include +#include +#include +#include #include #include -#include -#include -#include -#include -#include -#include -#include -#include +#include +#include +#include +#include +#include +#include +#include +#include -namespace svo { +namespace vio { int Seed::batch_counter = 0; int Seed::seed_counter = 0; @@ -354,4 +354,4 @@ double DepthFilter::computeTau( return (t_norm*sin(beta_plus)/sin(PI-acos(f.dot(t)/t_norm)-beta_plus) - z); // ( triangle angles sum to PI and law of sines ) tau } -} // namespace svo +} // namespace vio diff --git a/GPU_version/vio/src/feature_alignment.cpp b/GPU_version/vio/src/feature_alignment.cpp index 918ab25..0d8b455 100644 --- a/GPU_version/vio/src/feature_alignment.cpp +++ b/GPU_version/vio/src/feature_alignment.cpp @@ -20,9 +20,9 @@ #ifdef __ARM_NEON__ #include #endif -#include +#include -namespace svo { +namespace vio { namespace feature_alignment { #define SUBPIX_VERBOSE 0 @@ -613,4 +613,4 @@ bool align2D_NEON ( } } // namespace feature_alignment -} // namespace svo +} // namespace vio diff --git a/GPU_version/vio/src/feature_detection.cpp b/GPU_version/vio/src/feature_detection.cpp index 333e602..d19b427 100644 --- a/GPU_version/vio/src/feature_detection.cpp +++ b/GPU_version/vio/src/feature_detection.cpp @@ -14,11 +14,11 @@ // You should have received a copy of the GNU General Public License // along with this program. If not, see . -#include -#include -#include +#include +#include +#include -namespace svo { +namespace vio { namespace feature_detection { AbstractDetector::AbstractDetector( @@ -79,7 +79,7 @@ void FastDetector::detect( gpu_fast_->reload_buf(0,0,img); int icorner[1]={0}; gpu_fast_->reload_buf(0,2,icorner); - gpu_fast_->run(0,img_pyr[L].cols,img_pyr[L].rows); + gpu_fast_->run(0,img.cols,img.rows); cl_int2 fast_corners[350000]; gpu_fast_->read(0,1,fast_corners); int count[1]={0}; @@ -110,5 +110,5 @@ void FastDetector::detect( } } // namespace feature_detection -} // namespace svo +} // namespace vio diff --git a/GPU_version/vio/src/frame.cpp b/GPU_version/vio/src/frame.cpp index 30f085f..4bb2b9b 100644 --- a/GPU_version/vio/src/frame.cpp +++ b/GPU_version/vio/src/frame.cpp @@ -15,14 +15,14 @@ // along with this program. If not, see . #include -#include -#include -#include -#include -#include -#include +#include +#include +#include +#include +#include +#include -namespace svo { +namespace vio { int Frame::frame_counter_ = 0; @@ -186,4 +186,4 @@ bool getSceneDepth(const Frame& frame, double& depth_mean, double& depth_min) } } // namespace frame_utils -} // namespace svo +} // namespace vio diff --git a/GPU_version/vio/src/frame_handler_base.cpp b/GPU_version/vio/src/frame_handler_base.cpp index a232ce3..9f41cbc 100644 --- a/GPU_version/vio/src/frame_handler_base.cpp +++ b/GPU_version/vio/src/frame_handler_base.cpp @@ -14,19 +14,19 @@ // You should have received a copy of the GNU General Public License // along with this program. If not, see . -#include +#include #include #include #include #include -#include -#include -#include -#include -#include -#include +#include +#include +#include +#include +#include +#include -namespace svo +namespace vio { // definition of global and static variables which were declared in the header @@ -137,4 +137,4 @@ void FrameHandlerBase::optimizeStructure( } -} // namespace svo +} // namespace vio diff --git a/GPU_version/vio/src/frame_handler_mono.cpp b/GPU_version/vio/src/frame_handler_mono.cpp index 6399756..84cb7b3 100644 --- a/GPU_version/vio/src/frame_handler_mono.cpp +++ b/GPU_version/vio/src/frame_handler_mono.cpp @@ -14,18 +14,19 @@ // You should have received a copy of the GNU General Public License // along with this program. If not, see . -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include -namespace svo { +namespace vio { FrameHandlerMono::FrameHandlerMono(vk::AbstractCamera* cam,Eigen::Matrix& init) : FrameHandlerBase(), @@ -35,15 +36,33 @@ FrameHandlerMono::FrameHandlerMono(vk::AbstractCamera* cam,Eigen::Matrixmake_kernel("fast_gray"); cv::Mat img=cv::Mat(cv::Size(700, 500),CV_8UC1); gpu_fast_->write_buf(0,0,img); - cl_int2 corners_[350000]; + cl_int2 corners_[350000]={0}; gpu_fast_->write_buf(0,1,350000,corners_); int icorner[1]={0}; gpu_fast_->write_buf(0,2,1,icorner); gpu_fast_->write_buf(0,3,(1 << 18)); + gpu_fast_->make_kernel("compute_residual"); + gpu_fast_->write_buf(1,0,img); + gpu_fast_->write_buf(1,1,img); + cl_double3 pose[1]={0.0}; + gpu_fast_->write_buf(1,2,3,pose); + gpu_fast_->write_buf(1,3,3,pose); + cl_double3 f[300]={0}; + gpu_fast_->write_buf(1,4,300,f); + cl_double2 px[300]={0}; + gpu_fast_->write_buf(1,5,300,px); + double e[300]={0}; + gpu_fast_->write_buf(1,7,300,e); + double H[9*300]={0}; + gpu_fast_->write_buf(1,8,9*300,H); + cl_double3 J[300]; + gpu_fast_->write_buf(1,9,300,J); + double chi2[1]={0.01}; + gpu_fast_->write_buf(1,10,1,chi2); klt_homography_init_=new initialization::KltHomographyInit(gpu_fast_); initialize(); } @@ -140,10 +159,15 @@ FrameHandlerBase::UpdateResult FrameHandlerMono::processFrame() new_frame_->T_f_w_=init_f.second; new_frame_->Cov_ = init_f.first; // sparse image align - SparseImgAlign img_align(Config::kltMaxLevel(), Config::kltMinLevel(), - 30, SparseImgAlign::LevenbergMarquardt, false, false); + //SparseImgAlign img_align(Config::kltMaxLevel(), Config::kltMinLevel(),30, SparseImgAlign::LevenbergMarquardt, false, false); + SparseImgAlignGpu img_align(Config::kltMaxLevel(), Config::kltMinLevel(),30, SparseImgAlignGpu::GaussNewton, false,gpu_fast_); if(img_align.run(last_frame_, new_frame_)==0)return RESULT_FAILURE; + //assert (false); reprojector_.reprojectMap(new_frame_, overlap_kfs_); + std::cout<<"Reprojection Map nPoint: "<T_f_w_.se2().translation()-new_frame_->T_f_w_.se2().translation()).norm()<<'\n'; size_t sfba_n_edges_final=0; double sfba_thresh, sfba_error_init, sfba_error_final; pose_optimizer::optimizeGaussNewton( @@ -299,4 +323,4 @@ void FrameHandlerMono::UpdateCmd(double* value,const ros::Time& time){ ukfPtr_.UpdateCmd(value[0],value[1],value[2],time); } -} // namespace svo +} // namespace vio diff --git a/GPU_version/vio/src/homography.cpp b/GPU_version/vio/src/homography.cpp index c73617d..4044496 100644 --- a/GPU_version/vio/src/homography.cpp +++ b/GPU_version/vio/src/homography.cpp @@ -12,7 +12,7 @@ * by: cforster */ -#include +#include #include #include diff --git a/GPU_version/vio/src/initialization.cpp b/GPU_version/vio/src/initialization.cpp index 379163f..8149b85 100644 --- a/GPU_version/vio/src/initialization.cpp +++ b/GPU_version/vio/src/initialization.cpp @@ -14,16 +14,16 @@ // You should have received a copy of the GNU General Public License // along with this program. If not, see . -#include -#include -#include -#include -#include -#include -#include -#include +#include +#include +#include +#include +#include +#include +#include +#include -namespace svo { +namespace vio { namespace initialization { InitResult KltHomographyInit::addFirstFrame(FramePtr frame_ref) @@ -197,4 +197,4 @@ void computeHomography( } // namespace initialization -} // namespace svo +} // namespace vio diff --git a/GPU_version/vio/src/map.cpp b/GPU_version/vio/src/map.cpp index 9e4196e..b4734b3 100644 --- a/GPU_version/vio/src/map.cpp +++ b/GPU_version/vio/src/map.cpp @@ -15,13 +15,13 @@ // along with this program. If not, see . #include -#include -#include -#include -#include +#include +#include +#include +#include #include -namespace svo { +namespace vio { Map::Map() {} @@ -376,4 +376,4 @@ void mapStatistics(Map* map) } } // namespace map_debug -} // namespace svo +} // namespace vio diff --git a/GPU_version/vio/src/matcher.cpp b/GPU_version/vio/src/matcher.cpp index 1b025c9..ec16cd3 100644 --- a/GPU_version/vio/src/matcher.cpp +++ b/GPU_version/vio/src/matcher.cpp @@ -15,18 +15,18 @@ // along with this program. If not, see . #include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include -namespace svo { +namespace vio { namespace warp { @@ -324,4 +324,4 @@ bool Matcher::findEpipolarMatchDirect( return false; } -} // namespace svo +} // namespace vio diff --git a/GPU_version/vio/src/math_utils.cpp b/GPU_version/vio/src/math_utils.cpp index 9c265ec..bcaff29 100644 --- a/GPU_version/vio/src/math_utils.cpp +++ b/GPU_version/vio/src/math_utils.cpp @@ -5,7 +5,7 @@ * Author: cforster */ -#include +#include namespace vk { diff --git a/GPU_version/vio/src/output_helper.cpp b/GPU_version/vio/src/output_helper.cpp index 58aa0bf..cfb2cad 100644 --- a/GPU_version/vio/src/output_helper.cpp +++ b/GPU_version/vio/src/output_helper.cpp @@ -5,7 +5,7 @@ * Author: chrigi */ -#include +#include #include diff --git a/GPU_version/vio/src/point.cpp b/GPU_version/vio/src/point.cpp index cf12a1e..adb2c2b 100644 --- a/GPU_version/vio/src/point.cpp +++ b/GPU_version/vio/src/point.cpp @@ -15,12 +15,12 @@ // along with this program. If not, see . #include -#include -#include -#include -#include +#include +#include +#include +#include -namespace svo { +namespace vio { int Point::point_counter_ = 0; @@ -175,4 +175,4 @@ void Point::optimize(const size_t n_iter) #endif } -} // namespace svo +} // namespace vio diff --git a/GPU_version/vio/src/pose_optimizer.cpp b/GPU_version/vio/src/pose_optimizer.cpp index 3d45ab5..7ef9541 100644 --- a/GPU_version/vio/src/pose_optimizer.cpp +++ b/GPU_version/vio/src/pose_optimizer.cpp @@ -15,15 +15,15 @@ // along with this program. If not, see . #include -#include -#include -#include -#include -#include -#include +#include +#include +#include +#include +#include +#include #include -namespace svo { +namespace vio { namespace pose_optimizer { void optimizeGaussNewton( @@ -175,4 +175,4 @@ void optimizeGaussNewton( } } // namespace pose_optimizer -} // namespace svo +} // namespace vio diff --git a/GPU_version/vio/src/reprojector.cpp b/GPU_version/vio/src/reprojector.cpp index 897ce59..15bda16 100644 --- a/GPU_version/vio/src/reprojector.cpp +++ b/GPU_version/vio/src/reprojector.cpp @@ -16,19 +16,19 @@ #include #include -#include -#include -#include -#include -#include -#include +#include +#include +#include +#include +#include +#include #include #include -#include -#include -#include +#include +#include +#include -namespace svo { +namespace vio { Reprojector::Reprojector(vk::AbstractCamera* cam, Map& map) : map_(map) @@ -216,4 +216,4 @@ bool Reprojector::reprojectPoint(FramePtr frame, Point* point) return false; } -} // namespace svo +} // namespace vio diff --git a/GPU_version/vio/src/robust_cost.cpp b/GPU_version/vio/src/robust_cost.cpp index ec771ae..0090a69 100644 --- a/GPU_version/vio/src/robust_cost.cpp +++ b/GPU_version/vio/src/robust_cost.cpp @@ -20,8 +20,8 @@ #include #include -#include -#include +#include +#include namespace vk { namespace robust_cost { diff --git a/GPU_version/vio/src/sparse_img_align.cpp b/GPU_version/vio/src/sparse_img_align.cpp index 59c754c..128be16 100644 --- a/GPU_version/vio/src/sparse_img_align.cpp +++ b/GPU_version/vio/src/sparse_img_align.cpp @@ -15,16 +15,16 @@ // along with this program. If not, see . #include -#include -#include -#include -#include -#include -#include -#include -#include +#include +#include +#include +#include +#include +#include +#include +#include -namespace svo { +namespace vio { SparseImgAlign::SparseImgAlign( int max_level, int min_level, int n_iter, @@ -37,7 +37,7 @@ SparseImgAlign::SparseImgAlign( n_iter_init_ = n_iter_; method_ = method; verbose_ = verbose; - eps_ = 0.0001; + eps_ = 1e-5; } size_t SparseImgAlign::run(FramePtr ref_frame, FramePtr cur_frame) @@ -99,8 +99,8 @@ void SparseImgAlign::precomputeReferencePatches() *visiblity_it = true; // cannot just take the 3d points coordinate because of the reprojection errors in the reference image!!! - const double depth(((*it)->point->pos_ - ref_pos).norm()); - const Vector3d xyz_ref((*it)->f*depth); + // const double depth(((*it)->point->pos_ - ref_pos).norm()); + const Vector3d xyz_ref((*it)->f*((*it)->point->pos_ - ref_pos).norm()); // evaluate projection jacobian Matrix frame_jac; @@ -171,8 +171,8 @@ double SparseImgAlign::computeResiduals( continue; // compute pixel location in cur img - const double depth = ((*it)->point->pos_ - ref_pos).norm(); - const Vector3d xyz_ref((*it)->f*depth); + //const double depth = ((*it)->point->pos_ - ref_pos).norm(); + const Vector3d xyz_ref((*it)->f*((*it)->point->pos_ - ref_pos).norm()); const Vector3d xyz_cur((T_cur_from_ref.rotation_matrix()(0,0)*xyz_ref.x())+(T_cur_from_ref.rotation_matrix()(0,1)*xyz_ref.z())+T_cur_from_ref.translation()(0), xyz_ref.y(), -1.0*(T_cur_from_ref.rotation_matrix()(1,0)*xyz_ref.x())+(T_cur_from_ref.rotation_matrix()(1,1)*xyz_ref.z())+T_cur_from_ref.translation()(1)); @@ -224,8 +224,6 @@ double SparseImgAlign::computeResiduals( const Vector3d J(jacobian_cache_.col(feature_counter*patch_area_ + pixel_counter)); H_.noalias() += J*J.transpose()*weight; Jres_.noalias() -= J*res*weight; - if(display_) - resimg_.at((int) v_cur+y-patch_halfsize_, (int) u_cur+x-patch_halfsize_) = res/255.0; } } } @@ -265,5 +263,5 @@ void SparseImgAlign::finishIteration() } } -} // namespace svo +} // namespace vio diff --git a/GPU_version/vio/src/sparse_img_align_gpu.cpp b/GPU_version/vio/src/sparse_img_align_gpu.cpp new file mode 100644 index 0000000..431e041 --- /dev/null +++ b/GPU_version/vio/src/sparse_img_align_gpu.cpp @@ -0,0 +1,156 @@ +// 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+"); +} + +size_t SparseImgAlignGpu::run(FramePtr ref_frame, FramePtr cur_frame) +{ + reset(); + if(ref_frame->fts_.empty()) + { + return 0; + } + cl_double3 ref_pos[1]={ref_frame->pos()(0),ref_frame->pos()(1),ref_frame->T_f_w_.pitch()}; + cl_double3 cur_pos[1]={cur_frame->pos()(0),cur_frame->pos()(1),cur_frame->T_f_w_.pitch()}; + residual_->reload_buf(1,2,cur_pos); + residual_->reload_buf(1,3,ref_pos); + cl_double3 features[ref_frame->fts_.size()]; + cl_double2 featue_px[ref_frame->fts_.size()]; + // fprintf(data, "ref_frame:\n %f,%f,%f\n",ref_pos[0],ref_pos[1],ref_pos[2]); + // fprintf(data, "cur_frame:\n %f,%f,%f\n",cur_pos[0],cur_pos[1],cur_pos[2]); + // fprintf(data, "camera:\n %f,%f,%f,%f,%f\n features:\n",camera[0],camera[1],camera[2],camera[3],camera[4]); + feature_counter_ = 0; // is used to compute the index of the cached jacobian + for(auto it=ref_frame->fts_.begin(); it!=ref_frame->fts_.end();++it){ + if(*it == nullptr)continue; + if((*it)->point == nullptr)continue; + Vector3d xyz_ref=(*it)->f*((*it)->point->pos_ - Eigen::Vector3d(ref_pos[0].x,0.0,ref_pos[0].y)).norm(); + features[feature_counter_].x=xyz_ref(0); + features[feature_counter_].y=xyz_ref(1); + features[feature_counter_].z=xyz_ref(2); + featue_px[feature_counter_].x=(*it)->px(0); + featue_px[feature_counter_].y=(*it)->px(1); + // fprintf(data, "%f,%f,%f,%f,%f\n",xyz_ref(0),xyz_ref(1),xyz_ref(2),(*it)->px(0),(*it)->px(1)); + ++feature_counter_; + } + residual_->reload_buf(1,4,features); + residual_->reload_buf(1,5,featue_px); + SE2 T_cur(cur_frame->T_f_w_.se2());///TODO temporary, we can remove it + for(level_=max_level_; level_>=min_level_; --level_) + { + cv::Mat cur_img = cur_frame->img_pyr_.at(level_); + cv::Mat ref_img = ref_frame->img_pyr_.at(level_); + // cv::imwrite(std::string(getenv("HOME"))+"/Project/cur"+to_string(level_)+".png",cur_img); + // cv::imwrite(std::string(getenv("HOME"))+"/Project/fer"+to_string(level_)+".png",ref_img); + residual_->reload_buf(1,0,cur_img); + residual_->reload_buf(1,1,ref_img); + residual_->write_buf(1,6,level_); + mu_ = 1.0; + optimize(T_cur); + } + cl_double3 pos[1]={0}; + residual_->read(0,2,pos); + cur_frame->T_f_w_ = SE2_5(pos[0].x,pos[0].y,pos[0].z); + return 1; +} + +double SparseImgAlignGpu::computeResiduals( + bool linearize_system, + bool compute_weight_scale) +{ + double H[9*300]={0}; + cl_double3 J[300]={0.0}; + double chi2_[300]={0.0}; + residual_->write_buf(1,11,scale_); + residual_->reload_buf(1,10,chi2_); + residual_->reload_buf(1,8,H); + residual_->reload_buf(1,9,J); + /// TODO GPU compute based on the feature + residual_->run(1,feature_counter_); + float error[300]={0.0}; + double chi[300]={0.0}; + residual_->read(1,10,feature_counter_,chi); + residual_->read(1,7,feature_counter_,error); + for(int i = 1; i < feature_counter_; ++i){ + error[0] += error[i]; + chi[0] += chi[i]; + } + scale_ = error[0]*(1.48f / feature_counter_); + return chi[0]/(feature_counter_*8); +} + +int SparseImgAlignGpu::solve() +{ + double H[300*9]={0.0}; + cl_double3 J[300]={0.0}; + residual_->read(1,8,feature_counter_*9,H); + residual_->read(1,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; + } + x_ = Eigen::Matrix(H).ldlt().solve(Eigen::Vector3d(J[0].x,J[0].y,J[0].z)); + if((bool) std::isnan((double) x_[0])) + return 0; + return 1; +} +void SparseImgAlignGpu::update() +{ +/// TODO the update situation may have a smarter solution + cl_double3 pos[1]={0.0,0.0,0.0}; + residual_->read(1,2,pos); + Sophus::SE2 update = Sophus::SE2(pos[0].z,Eigen::Vector2d(pos[0].x,pos[0].y)) * Sophus::SE2::exp(-0.5*x_); + pos[0].x=update.translation()(0); + pos[0].y=update.translation()(1); + pos[0].z=atan(update.so2().unit_complex().imag()/update.so2().unit_complex().real()); + residual_->reload_buf(1,2,pos); +} + +void SparseImgAlignGpu::startIteration() +{} + +void SparseImgAlignGpu::finishIteration() +{ +} + +} // namespace vio + diff --git a/GPU_version/vio/src/vision.cpp b/GPU_version/vio/src/vision.cpp index e1be729..1b93692 100644 --- a/GPU_version/vio/src/vision.cpp +++ b/GPU_version/vio/src/vision.cpp @@ -5,7 +5,7 @@ * Author: cforster */ -#include +#include #if __SSE2__ # include diff --git a/GPU_version/vio/src/visualizer.cpp b/GPU_version/vio/src/visualizer.cpp index 0434f0f..279d73c 100644 --- a/GPU_version/vio/src/visualizer.cpp +++ b/GPU_version/vio/src/visualizer.cpp @@ -14,19 +14,19 @@ // You should have received a copy of the GNU General Public License // along with this program. If not, see . -#include -#include -#include -#include -#include +#include +#include +#include +#include +#include #include #include -#include -#include +#include +#include #include -#include +#include -namespace svo { +namespace vio { Visualizer:: Visualizer() : @@ -77,4 +77,4 @@ void Visualizer::publishMinimal( } } -} // end namespace gpu_svo +} // end namespace vio diff --git a/GPU_version/vio/src/vo_node.cpp b/GPU_version/vio/src/vo_node.cpp index a182007..2060ea0 100644 --- a/GPU_version/vio/src/vo_node.cpp +++ b/GPU_version/vio/src/vo_node.cpp @@ -17,8 +17,8 @@ #include #include #include -#include -#include +#include +#include #include #include #include @@ -26,21 +26,21 @@ #include #include #include -#include -#include -#include +#include +#include +#include #include #include #include -#include +#include #if VIO_DEBUG -#include +#include #endif class VioNode { public: - svo::FrameHandlerMono* vo_; + vio::FrameHandlerMono* vo_; vk::AbstractCamera* cam_; VioNode(); ~VioNode(); @@ -54,7 +54,7 @@ public: bool start(vio::start::Request& req, vio::start::Response& res){ if(req.on==1){ start_=true; - if(vo_->stage()!=svo::FrameHandlerBase::STAGE_DEFAULT_FRAME) + if(vo_->stage()!=vio::FrameHandlerBase::STAGE_DEFAULT_FRAME) vo_->start(); else{ vo_->depthFilter()->startThread(); @@ -106,7 +106,7 @@ VioNode::VioNode() : throw std::runtime_error("Camera model not correctly specified."); Eigen::Matrix init; init<<1e-19,1e-19,1e-19; - vo_ = new svo::FrameHandlerMono(cam_,init); + vo_ = new vio::FrameHandlerMono(cam_,init); usleep(500); } diff --git a/GPU_version/vio/test/test_pipeline.cpp b/GPU_version/vio/test/test_pipeline.cpp index ede3b25..4365e8b 100644 --- a/GPU_version/vio/test/test_pipeline.cpp +++ b/GPU_version/vio/test/test_pipeline.cpp @@ -30,7 +30,7 @@ #include #include "test_utils.h" -namespace svo { +namespace vio { class BenchmarkNode { @@ -84,7 +84,7 @@ void BenchmarkNode::runFromFolder() } } -} // namespace svo +} // namespace vio int main(int argc, char** argv) { diff --git a/GPU_version/vio/test/test_pose_optimizer.cpp b/GPU_version/vio/test/test_pose_optimizer.cpp index 59e0b46..45118eb 100644 --- a/GPU_version/vio/test/test_pose_optimizer.cpp +++ b/GPU_version/vio/test/test_pose_optimizer.cpp @@ -29,7 +29,7 @@ namespace { -using namespace svo; +using namespace vio; using namespace Eigen; /// Pose Optimizer Test-Fixture diff --git a/GPU_version/vio/test/test_sparse_img_align.cpp b/GPU_version/vio/test/test_sparse_img_align.cpp index 1312da0..521c2ba 100644 --- a/GPU_version/vio/test/test_sparse_img_align.cpp +++ b/GPU_version/vio/test/test_sparse_img_align.cpp @@ -14,18 +14,17 @@ // You should have received a copy of the GNU General Public License // along with this program. If not, see . -#include -#include -#include -#include -#include +#include +#include +#include +#include #include -#include -#include -#include -#include -#include -#include +#include +#include +#include +#include +#include +#include #include "test_utils.h" namespace { diff --git a/GPU_version/vio/test/test_utils.h b/GPU_version/vio/test/test_utils.h index 2759fe0..6de377a 100644 --- a/GPU_version/vio/test/test_utils.h +++ b/GPU_version/vio/test/test_utils.h @@ -24,7 +24,7 @@ # include #endif -namespace svo { +namespace vio { namespace test_utils { std::string getDatasetDir() @@ -51,7 +51,7 @@ std::string getTraceDir() } } // namespace test_utils -} // namespace svo +} // namespace vio #endif // TEST_UTILS_H_