optimized gpu

CPU_version/vio_svo/launch/Px30.launch

@@ -10,7 +10,7 @@
     <rosparam file="$(find vio_svo)/param/px30.yaml" />
 
     <!-- Initial camera orientation, make it point downwards -->
-    <param name="init_rx" value="3.14" />
+    <param name="init_rx" value="-2.35619" />
     <param name="init_ry" value="0.00" />
     <param name="init_rz" value="0.00" />
 

GPU_version/svo/include/gpu_svo/cl_class.h

@@ -4,7 +4,7 @@
 
 #ifndef VIO_OPENCL_CL_CLASS_H
 #define VIO_OPENCL_CL_CLASS_H
-#include <CL/cl.hpp>
+#include <gpu_svo/cl.hpp>
 #include <iostream>
 #include <vector>
 #include <algorithm>

GPU_version/svo/include/gpu_svo/config.h

@@ -122,17 +122,17 @@ public:
   /// acc white noise in continuous.
   static double& ACC_Noise() { return getInstance().ACC_noise; }
 
-  /// acc white noise in continuous.
+  /// Gro white noise in continuous.
   static double& GYO_Noise() { return getInstance().GYO_noise; }
   /// integration uncertainty continuous
   static double& IUC() { return getInstance().iuc; }
-    // acc bias in continuous
+    /// acc bias in continuous
   static double& ABC() { return getInstance().abc; }
-    // gyro bias in continuous
+    /// gyro bias in continuous
   static double& GBC() { return getInstance().gbc; }
-    // error in the bias used for preintegration
+    /// error in the bias used for preintegration
   static double& EBP() { return getInstance().ebp; }
-    // the noise used for projection
+    /// the noise used for projection
   static double& NRP() { return getInstance().nrp; }
 
 

GPU_version/svo/kernel/fast-gray.cl

@@ -1,6 +1,9 @@
 // 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
+
 __kernel void fast_gray(
     __read_only  image2d_t   image,
     __global     int2      * corners,

GPU_version/svo/src/feature_detection.cpp

@@ -80,7 +80,7 @@ void FastDetector::detect(
     int icorner[1]={0};
     gpu_fast_->reload_buf(0,2,icorner);
     gpu_fast_->run(0,img_pyr[L].cols,img_pyr[L].rows);
-    cl_int2 fast_corners[1000000];
+    cl_int2 fast_corners[350000];
     gpu_fast_->read(0,1,fast_corners);
     int count[1]={0};
     gpu_fast_->read(0,2,count);
@@ -97,18 +97,15 @@ void FastDetector::detect(
 
       const float score = vk::shiTomasiScore(img_pyr[L], fast_corners[i].x, fast_corners[i].y);
 
-      if(score > corners.at(k).score)
-        corners.at(k) = Corner(fast_corners[i].x*scale, fast_corners[i].y*scale, score, L, 0.0f);
+      if(score > corners.at(k).score){
+          corners.at(k) = Corner(fast_corners[i].x*scale, fast_corners[i].y*scale, score, L, 0.0f);
+          // Create feature for every corner that has high enough corner score
+          if(corners.at(k).score > detection_threshold)
+              fts.push_back(new Feature(frame, Vector2d(corners.at(k).x, corners.at(k).y), corners.at(k).level));
+      }
     }
 
   }
-
-  // Create feature for every corner that has high enough corner score
-  std::for_each(corners.begin(), corners.end(), [&](Corner& c) {
-    if(c.score > detection_threshold)
-      fts.push_back(new Feature(frame, Vector2d(c.x, c.y), c.level));
-  });
-
   resetGrid();
 }
 

GPU_version/svo/src/frame_handler_mono.cpp

@@ -38,10 +38,10 @@ FrameHandlerMono::FrameHandlerMono(vk::AbstractCamera* cam,Sophus::SE3& SE_init)
 {
     gpu_fast_= new opencl();
     gpu_fast_->make_kernel("fast_gray");
-    cv::Mat img=cv::Mat(cv::Size(1000, 1000),CV_8UC1);
+    cv::Mat img=cv::Mat(cv::Size(700, 500),CV_8UC1);
     gpu_fast_->write_buf(0,0,img);
-    cl_int2 corners_[1000000];
-    gpu_fast_->write_buf(0,1,1000000,corners_);
+    cl_int2 corners_[350000];
+    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_version/svo/src/imu_integration.cpp

@@ -111,6 +111,8 @@ bool Imu_Integration::predict(boost::shared_ptr<svo::Frame>& new_frame,std::size
     }else{
         if(imu_n<1)return false;
         auto preint_imu_combined =dynamic_cast<const gtsam::PreintegratedCombinedMeasurements&>(*preintegratedPtr);
+        //preint_imu_combined.predict(*statePtr,*imu_biasPtr).print();
+        //std::cerr<<new_frame->T_f_w_.matrix();
         valuesPtr->insert(P(imu_factor_id), gtsam::Pose3(new_frame->T_f_w_.matrix()));
         valuesPtr->insert(V(imu_factor_id), statePtr->v());
         valuesPtr->insert(B(imu_factor_id), *imu_biasPtr);

GPU_version/vio/launch/Px30.launch

@@ -10,9 +10,9 @@
     <rosparam file="$(find vio_gpu)/param/px30.yaml" />
 
     <!-- Initial camera orientation, make it point downwards -->
-    <param name="init_rx" value="3.14" />
-    <param name="init_ry" value="0.00" />
-    <param name="init_rz" value="0.00" />
+    <param name="init_rx" value="-2.35619" /> <!-- 90 + tetha -->
+    <param name="init_ry" value="0.0" />
+    <param name="init_rz" value="0.0" />
 
   </node>