public override void func_hi_and_dhi_by_dxp_and_dhi_by_dyi(Vector yi, Vector xp) { // This function gives relative position of feature: also call this hR // (vector from camera to feature in robot frame) func_zeroedyi_and_dzeroedyi_by_dxp_and_dzeroedyi_by_dyi(yi, xp); // Project this from 3D into the 2D image using our camera Vector feature_3D_position = get_zeroedyiRES(); WideCamera cam = ((Wide_Camera_Point_Feature_Measurement_Model)wide_model).m_camera; hiRES = cam.Project(feature_3D_position); // And ask the camera what the Jacobian of this projection was MatrixFixed dhid_by_dzeroedyi = cam.ProjectionJacobian(); // Form the required Jacobians dhi_by_dxpRES = dhid_by_dzeroedyi * dzeroedyi_by_dxpRES; dhi_by_dyiRES = dhid_by_dzeroedyi * dzeroedyi_by_dyiRES; }