Предположим, что углы плоскости, которую вы хотите нарисовать, представляют собой векторы столбцов 3x1, a = [0 0 0] ', b = [w 0 0]', c = [wh 0] 'и d = [0 h 0].
Предположим, что предоставленная вами калибровочная матрица является A и состоит из матрицы вращения R = A (1: 3, 1: 3) и перевода T = A (1: 3, 4).
Нарисовать первый вид
Для каждой позы A_i с вращением R_i и трансляцией T_i преобразуйте каждый угол x_w (то есть a, b, c или d) плоскости в его координаты x_c в камере на
x_c = R_i * x_w + T_i
Затем нарисуйте плоскость с преобразованными углами.
Чтобы нарисовать камеру, ее центр проекции в координатах камеры равен [0 0 0] ', а ось x камеры равна [1 0 0]', ось y равна [0 1 0] ', а ось z равна [0 0 1] '.
Обратите внимание, что на чертеже ось Y камеры направлена вниз, поэтому вы можете захотеть применить дополнительное вращение ко всем вычисленным координатам путем умножения на B = [1 0 0; 0 0 1; 0 -1 0].
Нарисуйте второй вид
Рисование плоскости тривиально, так как мы находимся в мировых координатах. Просто нарисуйте плоскость, используя a, b, c и d.
Чтобы нарисовать камеры, каждый центр камеры имеет вид c = -R '* T. Оси камеры - это строки матрицы вращения R, поэтому, например, в предоставленной вами матрице ось X
[2.11e-001 -3.06e-001 -9.28e-001] '. Вы также можете нарисовать камеру, преобразовав каждую точку x_c, заданную в координатах камеры, в мировые координаты x_w с помощью x_w = R '* (x_c - T) и нарисовав ее.