void StereoPWP3D::ComputeJacobiansForEye(const cv::Mat &classification_image, boost::shared_ptr<Model> current_model, boost::shared_ptr<MonocularCamera> camera, cv::Matx<float, 7, 1> &jacobian, cv::Matx<float, 7, 7> &hessian_approx, float &error){ cv::Mat sdf_image, front_intersection_image, back_intersection_image; ProcessSDFAndIntersectionImage(current_model, camera, sdf_image, front_intersection_image, back_intersection_image); auto stereo_frame = boost::dynamic_pointer_cast<sv::StereoFrame>(frame_); float fg_area = 1.0f, bg_area = 1.0f; size_t contour_area = 0; //ComputeAreas(sdf_image, fg_area, bg_area, contour_area); float *sdf_im_data = (float *)sdf_image.data; float *front_intersection_data = (float *)front_intersection_image.data; float *back_intersection_data = (float *)back_intersection_image.data; for (int r = 5; r < classification_image.rows - 5; ++r){ for (int c = 5; c < classification_image.cols - 5; ++c){ int i = r*classification_image.cols + c; if (sdf_im_data[i] <= float(HEAVYSIDE_WIDTH) - 1e-1 && sdf_im_data[i] >= -float(HEAVYSIDE_WIDTH) + 1e-1){ //-log(H * P_f + (1-H) * P_b) error += GetErrorValue(classification_image, r, c, sdf_im_data[i], 1.0f, fg_area, bg_area); //P_f - P_b / (H * P_f + (1 - H) * P_b) const float region_agreement = GetRegionAgreement(classification_image, r, c, sdf_im_data[i], fg_area, bg_area); int shifted_i = i; //find the closest point on the contour if this point is outside the contour if (sdf_im_data[i] < 0.0) { int closest_r, closest_c; bool found = FindClosestIntersection(sdf_im_data, r, c, sdf_image.rows, sdf_image.cols, closest_r, closest_c); if (!found) continue; //should this be allowed to happen? shifted_i = closest_r * sdf_image.cols + closest_c; } cv::Matx<float, 1, 7> jacs; for (int j = 0; j < 7; ++j){ jacs(j) = 0.0f; } const float dsdf_dx = 0.5f*(sdf_im_data[r*classification_image.cols + (c + 1)] - sdf_im_data[r*classification_image.cols + (c - 1)]); const float dsdf_dy = 0.5f*(sdf_im_data[(r + 1)*classification_image.cols + c] - sdf_im_data[(r - 1)*classification_image.cols + c]); //update the jacobian if (camera == stereo_camera_->left_eye()) UpdateJacobian(region_agreement, sdf_im_data[i], dsdf_dx, dsdf_dy, camera->Fx(), camera->Fy(), front_intersection_image.at<cv::Vec3f>(shifted_i), back_intersection_image.at<cv::Vec3f>(shifted_i), current_model, jacs); else if (camera == stereo_camera_->right_eye()) UpdateJacobianRightEye(region_agreement, sdf_im_data[i], dsdf_dx, dsdf_dy, camera->Fx(), camera->Fy(), front_intersection_image.at<cv::Vec3f>(shifted_i), back_intersection_image.at<cv::Vec3f>(shifted_i), current_model, jacs); else throw std::runtime_error(""); jacobian += jacs.t(); hessian_approx += (jacs.t() * jacs); } } } }