Проекция трехмерной точки на изображение

Есть набор точек в трёхмерном пространстве. Необходимо сделать их проекцию на изображение. Известны внутренние и внешние параметры камеры, коэффициенты дисторсии. Использовал функцию из opencv cv::projectPoints, но почему получаю не совсем корректные результаты (координаты выходит за границы изображения). Поэтому решил написать самостоятельную реализацию этой функции, но не понимаю как это сделать (новичок в компьютерном зрении). Приветствуется любая помощь.