OptimizationFunctionPF update

prostsza wersja dopasowywania punktów (tylko do najbliższego sąsiada);
niestety nie mogę przetestować, bo całość się u mnie nie kompiluje (i
wynika to chyba nie tylko z problemów z PCL-em)