From 3c4561eed24889764a3da6c26a73d2ecd6bfd91f Mon Sep 17 00:00:00 2001
From: gfuse <majidgeravand68@gmail.com>
Date: Wed, 18 Aug 2021 08:27:45 +0200
Subject: [PATCH] edit cpu part

---
 GPU_version/vio/src/sparse_img_align_gpu.cpp | 26 ++++++++++----------
 GPU_version/vio/src/vo_node.cpp              |  2 +-
 2 files changed, 14 insertions(+), 14 deletions(-)

diff --git a/GPU_version/vio/src/sparse_img_align_gpu.cpp b/GPU_version/vio/src/sparse_img_align_gpu.cpp
index 5dd9a7a..4510998 100644
--- a/GPU_version/vio/src/sparse_img_align_gpu.cpp
+++ b/GPU_version/vio/src/sparse_img_align_gpu.cpp
@@ -40,8 +40,8 @@ size_t SparseImgAlignGpu::run(FramePtr ref_frame, FramePtr cur_frame)
   {
     return 0;
   }
-  cl_float3 ref_pos[1]={ref_frame->pos()(0),ref_frame->pos()(1),ref_frame->T_f_w_.pitch()};
-  cl_float3 cur_pos[1]={cur_frame->pos()(0),cur_frame->pos()(1),cur_frame->T_f_w_.pitch()};
+  cl_float3 ref_pos[1]={(float)ref_frame->pos()(0),(float)ref_frame->pos()(1),(float)ref_frame->T_f_w_.pitch()};
+  cl_float3 cur_pos[1]={(float)cur_frame->pos()(0),(float)cur_frame->pos()(1),(float)cur_frame->T_f_w_.pitch()};
   residual_->reload_buf(1,2,cur_pos);
   residual_->reload_buf(1,3,ref_pos);
   cl_float3 features[ref_frame->fts_.size()];
@@ -77,7 +77,7 @@ size_t SparseImgAlignGpu::run(FramePtr ref_frame, FramePtr cur_frame)
     mu_ = 1.0;
     optimize(T_cur);
   }
-  cl_double3 pos[1]={0};
+  cl_float3 pos[1]={0};
   residual_->read(1,2,pos);
   cur_frame->T_f_w_ = SE2_5(pos[0].x,pos[0].y,pos[0].z);
   return 1;
@@ -87,9 +87,9 @@ 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};
+    float H[9*300]={0};
+    cl_float3 J[300]={0.0};
+    float chi2_[300]={0.0};
     residual_->write_buf(1,11,scale_);
     residual_->reload_buf(1,10,chi2_);
     residual_->reload_buf(1,8,H);
@@ -97,7 +97,7 @@ double SparseImgAlignGpu::computeResiduals(
     /// TODO GPU compute based on the feature
     residual_->run(1,feature_counter_);
     float error[300]={0.0};
-    double chi[300]={0.0};
+    float 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){
@@ -110,8 +110,8 @@ double SparseImgAlignGpu::computeResiduals(
 
 int SparseImgAlignGpu::solve()
 {
-    double H[300*9]={0.0};
-    cl_double3 J[300]={0.0};
+    float H[300*9]={0.0};
+    cl_float3 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){
@@ -136,12 +136,12 @@ int SparseImgAlignGpu::solve()
 void SparseImgAlignGpu::update()
 {
 /// TODO the update situation may have a smarter solution
-    cl_double3 pos[1]={0.0,0.0,0.0};
+    cl_float3 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());
+    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(1,2,pos);
 }
 
diff --git a/GPU_version/vio/src/vo_node.cpp b/GPU_version/vio/src/vo_node.cpp
index 63e9de0..489d7f2 100644
--- a/GPU_version/vio/src/vo_node.cpp
+++ b/GPU_version/vio/src/vo_node.cpp
@@ -13,7 +13,7 @@
 //
 // You should have received a copy of the GNU General Public License
 // along with this program.  If not, see <http://www.gnu.org/licenses/>.
-#define VIO_DEBUG true
+#define VIO_DEBUG false
 #include <ros/ros.h>
 #include <string>
 #include <chrono>