change homography

GPU_version/vio/src/depth_filter.cpp

@@ -269,10 +269,7 @@ namespace vio {
 #if VIO_DEBUG
             fprintf(log_,"[%s]  If point is visible? %f, %f, %f\n",vio::time_in_HH_MM_SS_MMM().c_str(),xyz_f.x(),xyz_f.y(),xyz_f.z());
 #endif
-/*            if(xyz_f.z() > 0.0)  {
-                ++it; // behind the camera
-                continue;
-            }*/
+
             if(!frame->cam_->isInFrame(frame->f2c(xyz_f).cast<int>())) {
                 ++it; // point does not project in image
                 continue;
@@ -283,10 +280,6 @@ namespace vio {
 #if VIO_DEBUG
             fprintf(log_,"[%s]  Z inverse min: %f, Z inverse max: %f\n",vio::time_in_HH_MM_SS_MMM().c_str(),z_min,z_max);
 #endif
-/*            if(z_min < 0){
-                ++it;
-                continue;
-            }*/
 
             double z;
             if(!matcher_.findEpipolarMatchDirect(

GPU_version/vio/src/frame_handler_mono.cpp

@@ -57,13 +57,13 @@ FrameHandlerMono::FrameHandlerMono(vk::AbstractCamera* cam,Eigen::Matrix<double,
     errors.push_back(gpu_fast_->write_buf(1,4,300,f));
     cl_float2 px[300]={0};
     errors.push_back(gpu_fast_->write_buf(1,5,300,px));
-    float  e[300]={0};
+    cl_float  e[300]={0};
     errors.push_back(gpu_fast_->write_buf(1,7,300,e));
-    float H[9*300]={0};
+    cl_float H[9*300]={0};
     errors.push_back(gpu_fast_->write_buf(1,8,9*300,H));
     cl_float3 J[300];
     errors.push_back(gpu_fast_->write_buf(1,9,300,J));
-    float chi2[300]={0.01};
+    cl_float chi2[300]={0.01};
     errors.push_back(gpu_fast_->write_buf(1,10,300,chi2));
     for(auto e:errors){
         if(e!=true){

GPU_version/vio/src/homography.cpp

@@ -151,7 +151,7 @@ namespace vk {
             decomp.R(2,2) = dCosTheta;
 
             // Eq 14
-            decomp.t[0] = (d1 - d3) * x1_PM * e1[signs];
+            decomp.t[0] = 0.0;//(d1 - d3) * x1_PM * e1[signs];
             decomp.t[1] = 0.0;
             decomp.t[2] = (d1 - d3) * -x3_PM * e3[signs];
 
@@ -180,7 +180,7 @@ namespace vk {
             decomp.R(2,2) = -dCosPhi;
 
             // Eq 16
-            decomp.t[0] = (d1 + d3) * x1_PM * e1[signs];
+            decomp.t[0] = 0.0;//(d1 + d3) * x1_PM * e1[signs];
             decomp.t[1] = 0.0;
             decomp.t[2] = (d1 + d3) * x3_PM * e3[signs];
 

GPU_version/vio/src/initialization.cpp

@@ -58,7 +58,7 @@ InitResult KltHomographyInit::addSecondFrame(FramePtr frame_cur)
   double disparity = vk::getMean(disparities_,min,max);
     if(disparity < Config::initMinDisparity()){
         auto result=ukf_->get_location();
-        if(fabs(result.second.se2().translation().x())>0.02 || fabs(result.second.pitch())>0.0698132)ukf_->UpdateSvo(0.0,result.second.se2().translation().y(),0.0);
+        if(fabs(result.second.se2().translation().x())>0.2 || fabs(result.second.pitch())>0.0698132)ukf_->UpdateSvo(0.0,result.second.se2().translation().y(),0.0);
 #if VIO_DEBUG
         fprintf(log_,"[%s] Init: px average disparity is:%f ,While minimum is: %f  KLT tracked : %d\n",vio::time_in_HH_MM_SS_MMM().c_str(),
                 disparity,

GPU_version/vio/src/sparse_img_align_gpu.cpp

@@ -85,17 +85,17 @@ double SparseImgAlignGpu::computeResiduals(
     bool linearize_system,
     bool compute_weight_scale)
 {
-    float H[9*300]={0};
+    cl_float H[9*300]={0};
     cl_float3 J[300]={0.0};
-    float chi2_[300]={0.0};
-    float scale=(float)scale_;
+    cl_float chi2_[300]={0.0};
+    cl_float scale=(float)scale_;
     errors.push_back(residual_->write_buf(1,11,scale));
     errors.push_back(residual_->reload_buf(1,10,chi2_));
     errors.push_back(residual_->reload_buf(1,8,H));
     errors.push_back(residual_->reload_buf(1,9,J));
     residual_->run(1,feature_counter_);
-    float error[300]={0.0};
-    float chi[300]={0.0};
+    cl_float error[300]={0.0};
+    cl_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){
@@ -108,7 +108,7 @@ double SparseImgAlignGpu::computeResiduals(
 
 int SparseImgAlignGpu::solve()
 {
-    float H[300*9]={0.0};
+    cl_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);
@@ -130,6 +130,10 @@ int SparseImgAlignGpu::solve()
                   (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};
+    for(int i=0;i<9;++i){
+        if(i==0)std::cerr<<'\n'<<(double)J[0].x<<","<<(double)J[0].y<<","<<(double)J[0].z<<'\t';
+        std::cerr<<Hd[i]<<",";
+    }
     x_ = Eigen::Matrix<double,3,3>(Hd).ldlt().solve(Eigen::Vector3d(Jd[0],Jd[1],Jd[2]));
     if((bool) std::isnan((double) x_[0]) || (bool) std::isnan((double) x_[1]) ||(bool) std::isnan((double) x_[2]))
         return 0;