opencl kernel compute residual

GPU_version/vio/kernel/compute-residual.cl

@@ -32,40 +32,40 @@ double3 xyz_ref(double fts_pos_x,double fts_pos_y,double fts_pos_z,double ref_x,
     return double3(x, y, z);
 }
 
-void jacobian_xyz2uv(double3& xyz_in_f, double6& J)
+void jacobian_xyz2uv(double3& xyz_in_f, double* J)
 {
     double x = xyz_in_f[0];
     double y = xyz_in_f[1];
     double z_inv = 1. / xyz_in_f[2];
     double z_inv_2 = z_inv * z_inv;
 
-    J[0] = -z_inv;              // -1/z
+    *(J + 0) = -z_inv;              // -1/z
-    J[1] = x * z_inv_2;           // x/z^2
+    *(J + 1) = x * z_inv_2;           // x/z^2
-    J[2] = -(1.0 + pow(x,2) / z_inv_2);   // -(1.0 + x^2/z^2)
+    *(J + 2) = -(1.0 + pow(x,2) / z_inv_2);   // -(1.0 + x^2/z^2)
 
-    J[3] = 1e-19;                // 0
+    *(J + 3) = 1e-19;                // 0
-    J[4] = y * z_inv_2;           // y/z^2
+    *(J + 4) = y * z_inv_2;           // y/z^2
-    J[5] = -x * y / z_inv_2;      // -x*y/z^2
+    *(J + 5) = -x * y / z_inv_2;      // -x*y/z^2
 }
 
-void compute_hessain(double3& j, double9& H, double w)
+void compute_hessain(double3 j, double* H, double w)
 {
-    H[0] += j[0] * j[0] * w;
+    *(H + 0) += j[0] * j[0] * w;
-    H[1] += j[0] * j[1] * w;
+    *(H + 1) += j[0] * j[1] * w;
-    H[2] += j[0] * j[2] * w;
+    *(H + 2) += j[0] * j[2] * w;
-    H[3] += j[1] * j[0] * w;
+    *(H + 3) += j[1] * j[0] * w;
-    H[4] += j[1] * j[1] * w;
+    *(H + 4) += j[1] * j[1] * w;
-    H[5] += j[1] * j[2] * w;
+    *(H + 5) += j[1] * j[2] * w;
-    H[6] += j[2] * j[0] * w;
+    *(H + 6) += j[2] * j[0] * w;
-    H[7] += j[2] * j[1] * w;
+    *(H + 7) += j[2] * j[1] * w;
-    H[8] += j[2] * j[2] * w;
+    *(H + 8) += j[2] * j[2] * w;
 }
 
-void compute_Jacobian(double3& j, double& Jacobian, double r, double w)
+void compute_Jacobian(double3 j, double* Jacobian, double r, double w)
 {
-    Jacobian[0] -= j[0] * r * w;
+    *(Jacobian + 0) -= j[0] * r * w;
-    Jacobian[1] -= j[1] * r * w;
+    *(Jacobian + 1) -= j[1] * r * w;
-    Jacobian[2] -= j[2] * r * w;
+    *(Jacobian + 2) -= j[2] * r * w;
 }
 
 __kernel void compute-residual(
@@ -74,6 +74,7 @@ __kernel void compute-residual(
     __global     double      * cur_pose,//[reference frame pose{x,z,pitch}]
     __global     double      * 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,
     __global     int         * level,
     __global     double      * camera_model, // camera model [f_x,f_y,c_x,c_y,s_]
                  float       * errors,
@@ -100,8 +101,8 @@ __kernel void compute-residual(
     //ref_feature[f].y;
     //ref_feature[f].z;
 
-    double u_ref = ref_feature[f].x * scale; //??????  ref_feature[f].x
+    double u_ref = featue_px[f].x * scale;
-    double v_ref = ref_feature[f].y * scale; //??????  ref_feature[f].y
+    double v_ref = featue_px[f].y * scale;
     int u_ref_i = floor(u_ref);
     int v_ref_i = floor(v_ref);
 
@@ -112,7 +113,7 @@ __kernel void compute-residual(
 
     xyz_reference = xyz_ref(ref_feature[f][0], ref_feature[f][1], ref_feature[f][2], ref_pose.x, ref_pose.y, ref_pose.z); // ref_pose.y ????
     // evaluate projection jacobian
-    double6 frame_jac; //2X3
+    double* frame_jac = (double *) malloc(6 * sizeof(double)); // 2X3
     jacobian_xyz2uv(xyz_reference, frame_jac);
     double subpix_u_ref = u_ref - u_ref_i;
     double subpix_v_ref = v_ref - v_ref_i;
@@ -141,25 +142,34 @@ __kernel void compute-residual(
     double w_cur_tr = subpix_u_cur * (1.0 - subpix_v_cur);
     double w_cur_bl = (1.0 - subpix_u_cur) * subpix_v_cur;
     double w_cur_br = subpix_u_cur * subpix_v_cur;
-
-    /// TODO - check & implementation
-    // float* ref_patch_cache_ptr = reinterpret_cast<float*>(ref_patch_cache_.data) + patch_area_*feature_counter;
     /// ... compute residual
 
+    /// TODO - check & implementation
     // float* cache_ptr = reinterpret_cast<float*>(ref_patch_cache_.data) + patch_area_*feature_counter;
+    // float* ref_patch_cache_ptr = reinterpret_cast<float*>(ref_patch_cache_.data) + patch_area_*feature_counter;
+
+
     for(int y = 0; y < PATCH_SIZE; ++y)
     {
         /// TODO - check & implementation
+        int ref_element_add = image_ref.data + (v_ref_i + y - PATCH_HALFSIZE) * stride + (u_ref_i - PATCH_HALFSIZE);
+        int cur_element_add = image_cur.data + (v_cur_i + y - PATCH_HALFSIZE) * stride + (u_cur_i - PATCH_HALFSIZE);
         // uint8_t* ref_img_ptr = (uint8_t*) ref_img.data + (v_ref_i+y-patch_halfsize_)*stride + (u_ref_i-patch_halfsize_);
         // uint8_t* cur_img_ptr = (uint8_t*) image_cur.data + (v_cur_i + y - patch_halfsize_) * stride + (u_cur_i - patch_halfsize_);
 
         // for(int x = 0; x < PATCH_SIZE; ++x, ++pixel_counter, ++ref_img_ptr, ++cache_ptr)
         // for(int x = 0; x < PATCH_SIZE; ++x, ++pixel_counter, ++cur_img_ptr, ++ref_patch_cache_ptr)
-        for(int x = 0; x < PATCH_SIZE; ++x, ++pixel_counter, ++ref_img_ptr, ++cur_img_ptr)
+        for(int x = 0; x < PATCH_SIZE; ++x, ++pixel_counter, ++ref_element, ++cur_element)
         {
             /// TODO - check & implementation
             // precompute interpolated reference patch color
             // *cache_ptr = w_ref_tl*ref_img_ptr[0] + w_ref_tr*ref_img_ptr[1] + w_ref_bl*ref_img_ptr[stride] + w_ref_br*ref_img_ptr[stride+1];
+            int2 px_reftl = (ref_element_add % get_image_dim(image_ref)[0], ref_element_add / get_image_dim(image_ref)[0]);
+            int2 px_reftr = ((ref_element_add + 1) % get_image_dim(image_ref)[0], (ref_element_add + 1) / get_image_dim(image_ref)[0]);
+            int2 px_refbl = ((ref_element_add + stride) % get_image_dim(image_ref)[0], (ref_element_add + stride) / get_image_dim(image_ref)[0]);
+            int2 px_refbr = ((ref_element_add + stride + 1) % get_image_dim(image_ref)[0], (ref_element_add + stride + 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;
 
             // we use the inverse compositional: thereby we can take the gradient always at the same position
             // get gradient of warped image (~gradient at warped position)
@@ -171,10 +181,18 @@ __kernel void compute-residual(
             // compute residual
             /// TODO - check & implementation
             // double intensity_cur = w_cur_tl * cur_img_ptr[0] + w_cur_tr * cur_img_ptr[1] + w_cur_bl * cur_img_ptr[stride] + w_cur_br * cur_img_ptr[stride+1];
-            double res = intensity_cur - (ref_patch_cache_ptr[x]);
+            int2 px_curtl = (cur_element_add % get_image_dim(image_cur)[0], cur_element_add / get_image_dim(image_cur)[0]);
+            int2 px_curtr = ((cur_element_add + 1) % get_image_dim(image_cur)[0], (cur_element_add + 1) / get_image_dim(image_cur)[0]);
+            int2 px_curbl = ((cur_element_add + stride) % get_image_dim(image_cur)[0], (cur_element_add + stride) / get_image_dim(image_cur)[0]);
+            int2 px_curbr = ((cur_element_add + stride + 1) % get_image_dim(image_cur)[0], (cur_element_add + stride + 1) / get_image_dim(image_cur)[0]);
+            double intensity_cur = 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;
+
+            // const float res = intensity_cur - (*ref_patch_cache_ptr);
+            double res = intensity_cur - value;
 
             // used to compute scale for robust cost
-            if(compute_weight_scale)
+            if(compute_weight_scale)  //?????????  compute_weight_scale ????????????
             errors[(y * PATCH_SIZE) + x] = fabs(res)
 
             // robustification
@@ -199,5 +217,6 @@ __kernel void compute-residual(
             feature_counter++;
         }
     }
+
 }