function idx = findPointIndex(pointMap, point)
    % Find the index of a point in the pointMap
    keys = cell2mat(pointMap.keys);
    values = cell2mat(pointMap.values);
    for i = 1:length(keys)
        if isequal(values(:, i), point)
            idx = keys(i);
            return;
        end
    end
    error('Point not found in the map.');
end