56 lines
1.4 KiB
C
56 lines
1.4 KiB
C
|
// Copyright (c) Microsoft Corporation. All rights reserved.
|
||
|
|
// Licensed under the MIT License.
|
||
|
|
#pragma once
|
||
|
|
|
||
|
|
#include <opencv2/core.hpp>
|
||
|
|
|
||
|
|
struct Transformation
|
||
|
|
{
|
||
|
|
cv::Matx33d R;
|
||
|
|
cv::Vec3d t;
|
||
|
|
|
||
|
|
// Construct an identity transformation.
|
||
|
|
Transformation() : R(cv::Matx33d::eye()), t(0., 0., 0.) {}
|
||
|
|
|
||
|
|
// Construct from H
|
||
|
|
Transformation(const cv::Matx44d &H) : R(H.get_minor<3, 3>(0, 0)), t(H(0, 3), H(1, 3), H(2, 3)) {}
|
||
|
|
|
||
|
|
// Create homogeneous matrix from this transformation
|
||
|
|
cv::Matx44d to_homogeneous() const
|
||
|
|
{
|
||
|
|
return cv::Matx44d(
|
||
|
|
// row 1
|
||
|
|
R(0, 0),
|
||
|
|
R(0, 1),
|
||
|
|
R(0, 2),
|
||
|
|
t(0),
|
||
|
|
// row 2
|
||
|
|
R(1, 0),
|
||
|
|
R(1, 1),
|
||
|
|
R(1, 2),
|
||
|
|
t(1),
|
||
|
|
// row 3
|
||
|
|
R(2, 0),
|
||
|
|
R(2, 1),
|
||
|
|
R(2, 2),
|
||
|
|
t(2),
|
||
|
|
// row 4
|
||
|
|
0,
|
||
|
|
0,
|
||
|
|
0,
|
||
|
|
1);
|
||
|
|
}
|
||
|
|
|
||
|
|
// Construct a transformation equivalent to this transformation followed by the second transformation
|
||
|
|
Transformation compose_with(const Transformation &second_transformation) const
|
||
|
|
{
|
||
|
|
// get this transform
|
||
|
|
cv::Matx44d H_1 = to_homogeneous();
|
||
|
|
// get the transform to be composed with this one
|
||
|
|
cv::Matx44d H_2 = second_transformation.to_homogeneous();
|
||
|
|
// get the combined transform
|
||
|
|
cv::Matx44d H_3 = H_1 * H_2;
|
||
|
|
return Transformation(H_3);
|
||
|
|
}
|
||
|
|
};
|