Основная матрица оценивает движения по неправильной оси

Question

Я пытаюсь оценить позу монокулярной камеры, но когда я использую функцию recoveryPose, я получаю большинство движений вдоль оси z, в то время как я перемещаю камеру только горизонтально.В моей заявке нам нужно восстановить позу, чтобы мы могли сгенерировать трехмерную модель записанного сценария.

На самом деле мы используем камеру глубины для создания 3D-точек, но когда мы попытались использовать PnP для вычисления перевода, это привело к ужасным результатам.Поскольку основная матрица обеспечивает только унитарное смещение, мы вычислили коэффициент масштабирования, сравнив расстояния между двумя парами точек.

Чтобы накопить движение, мы умножили однородное преобразование между каждой последовательной позой камеры, а затем для каждого кадра мы умножили вектор XYZ1 на это однородное преобразование.

Мы попытались изменить алгоритм оценки позы на один из тех, которые встроены в функции solvePnP с датчиком глубины.Очевидно, что сопоставление функций довольно устойчиво, и, поскольку мы вычисляем их между последовательными кадрами потока изображения со скоростью 30 кадров в секунду.

При умножении преобразования я думаю, что матрица, заданная функцией recoveryPose, должна быть инвертирована, но ни в одном из случаев она не дает хороших результатов.

Извлечение функций

def find_correspondence(old_ip,old_features, new_ip, new_features):
    # BFMatcher with default params
    #Flag per si estem perduts, actualment es salta la foto i pasa a la seguent pero probablement funcioni millor
    # si tornem enrere i agafem el u~ltim frame amb una bona transformada com a nova referencia
    bf=cv.BFMatcher(cv.NORM_HAMMING, crossCheck=True)
    matches = bf.match(old_features,new_features)    #debugat una mica aquesta part, feia el tonto amb alguns vectors
    matches=sorted(matches, key = lambda x:x.distance)
    old_ip_loc= [old_ip[mat.queryIdx].pt for mat in matches]
    new_ip_loc= [new_ip[mat.trainIdx].pt for mat in matches]
    old_ip_loc=np.round(old_ip_loc)# without round
    new_ip_loc=np.round(new_ip_loc)# without round
    return old_ip_loc.astype(int),new_ip_loc.astype(int)

Вычисление необходимой матрицы

#find correspondence to the first image database with the last photo recorded
    points1,points2=find_correspondence(PointDatabase[counterframe-1],FeatureDatabase[counterframe-1],int_points, features)



if flag==False:# normal proces, the image is not required to be avoid
        #Esential Matrix
        E,mask=cv.findEssentialMat(points1, points2, focallength,PPCam, cv.RANSAC, 0.099, 1.0)
        #Get pose refered to the first photo database
        _, R, t, mask = cv.recoverPose(E,points1,points2,focal=focallength,pp=PPCam)
        #Save the pose matrix
        PoseNoScale=np.concatenate((R,t),1)

Вычисление коррекции

for var in range (0,len(points1commun)-1):

    Pospoint1btw21 = cv.triangulatePoints(Poses3x4[counterframe-1],Poses3x4[counterframe-2], points2commun[var], points1commun[var])
    Pospoint2btw21 = cv.triangulatePoints(Poses3x4[counterframe-1],Poses3x4[counterframe-2], points2commun[var+1], points1commun[var+1])
    Pospoint1btw23 = cv.triangulatePoints(Poses3x4[counterframe-1], PoseNoScale, points2commun[var], points3commun[var])
    Pospoint2btw23 = cv.triangulatePoints(Poses3x4[counterframe-1], PoseNoScale, points2commun[var+1], points3commun[var+1])

                Pospoint1btw21X.append(Pospoint1btw21[0]/Pospoint1btw21[3])
                Pospoint2btw21X.append(Pospoint2btw21[0]/Pospoint2btw21[3])
                Pospoint1btw23X.append(Pospoint1btw23[0]/Pospoint1btw23[3])
                Pospoint2btw23X.append(Pospoint2btw23[0]/Pospoint2btw23[3])
                #4 check if the distance between two keypoints remain the same if its the case, the scale is 1 otherwise correct the scale

ComputeScale.append(abs(scale*(Pospoint1btw23X[var]-Pospoint2btw23X[var])/((Pospoint1btw21X[var]-Pospoint2btw21X[var]))))

#5 Reject outliers
        ComputeScaleClean=[]
    meanscale=np.mean(ComputeScale)

    for var2 in range (0,len(ComputeScale)):
        #the scale has to be low
        if ComputeScale[var2]<meanscale:
#the points has to be more or less similar so de difference of distance is *10 or /10
                if ComputeScale[var2]>=0.1 and ComputeScale[var2]<=10 :
                        ComputeScaleClean.append(ComputeScale[var2])

    if len(ComputeScaleClean)>=1:
        scale=np.median(ComputeScaleClean)
    else:
        # reset scale
        scale=scale

Поскольку движение является горизонтальным, мы ожидали большой разброспо одной оси и близки к нулю вариации по оси Z, но на самом деле мы получили большие вариации по обеим x, z, что явно неверно.

Заранее спасибо!