Opencv solvePnpRansa c неправильная оценка позы - PullRequest
Новая
       111

Opencv solvePnpRansa c неправильная оценка позы

0 голосов
/ 04 марта 2020

Я работаю над графиком SLAM и оцениваю методы на наборе данных KITTI. Я использую метод openCv solvePnPRansa c, чтобы оценить позу заданных трехмерных точек и соответствующих им двухмерных проекций. В большинстве случаев он работает нормально, но в некоторых ситуациях метод оценивает неправильные позы, поскольку точки 3D-сцены не проецируются на соответствующие им 2D-координаты.

Примечание. На приведенных ниже изображениях показаны только внутренние 3D-точки и соответствующие им Возможности.

Результатом успешной оценки позы является изображение такого типа, в котором проекция точек трехмерной сцены (желтая звезда) с использованием оценочной матрицы камеры попадает в двумерные объекты (красный круг).

КАДР 274 enter image description here

FRAME290 enter image description here

FRAME 304 enter image description here

A неправильная оценка позы приводит к изображению ниже. Есть только 3 вкладыша (количество вкладышей обычно не слишком много). Все три точки 3D-сцены проецируются на одну и ту же 2D-функцию, что неверно.

КАДР 305 enter image description here

Я был бы очень признателен, если бы кто-то мог предложить решение.

Вот часть моей функции, которую я использую для оценки позы: rotVe c - это исходное предположение о ориентации в виде вектора вращения, а frame.position - начальное предположение о положении (оба получены из модели движения. ) 

    def refinePoseV2(self, frame, mapPoints, features):
        distCoeff = np.zeros((4,1))
        points3D = np.vstack([pt.position for pt in mapPoints])
        features2D = np.vstack([pt.feature.pt for pt in features])
        rotVec = Rotation.from_matrix(frame.orientation.R).as_rotvec()

        success, rotation_vector, translation_vector, inliers = cv.solvePnPRansac( points3D, features2D, 
                                                                            frame.cam.calibrationMatrix1, 
                                                                            distCoeff, reprojectionError=1,
                                                                            useExtrinsicGuess=True,
                                                                            rvec=rotVec, tvec=frame.position,
                                                                            flags=cv.SOLVEPNP_P3P)
...