/** 对输入点进行平移操作
*/
void Translate(const Mat3X&input_points, const Vec3 &vec_translation,
Mat3X *output_points) {
output_points->resize(input_points.rows(), input_points.cols());
for (size_t i = 0; i < input_points.cols(); ++i) {
output_points->col(i) = input_points.col(i) + vec_translation;
}
}
ResectionKernel_K(const Mat2X &x_image, const Mat3X &X, const Mat3 &K)
: x_image_(x_image), X_(X), K_(K)
{
assert(x_image.cols() == X.cols());
// Conversion from image coordinates to normalized camera coordinates
EuclideanToNormalizedCamera(x_image_, K, &x_camera_);
}
// Generates all necessary inputs and expected outputs for EuclideanResection.
void CreateCameraSystem(const Mat3& KK,
const Mat3X& x_image,
const Vec& X_distances,
const Mat3& R_input,
const Vec3& T_input,
Mat2X *x_camera,
Mat3X *X_world,
Mat3 *R_expected,
Vec3 *T_expected) {
int num_points = x_image.cols();
Mat3X x_unit_cam(3, num_points);
x_unit_cam = KK.inverse() * x_image;
// Create normalized camera coordinates to be used as an input to the PnP
// function, instead of using NormalizeColumnVectors(&x_unit_cam).
*x_camera = x_unit_cam.block(0, 0, 2, num_points);
for (int i = 0; i < num_points; ++i){
x_unit_cam.col(i).normalize();
}
// Create the 3D points in the camera system.
Mat X_camera(3, num_points);
for (int i = 0; i < num_points; ++i) {
X_camera.col(i) = X_distances(i) * x_unit_cam.col(i);
}
// Apply the transformation to the camera 3D points
Mat translation_matrix(3, num_points);
translation_matrix.row(0).setConstant(T_input(0));
translation_matrix.row(1).setConstant(T_input(1));
translation_matrix.row(2).setConstant(T_input(2));
*X_world = R_input * X_camera + translation_matrix;
// Create the expected result for comparison.
*R_expected = R_input.transpose();
*T_expected = *R_expected * ( - T_input);
};
void HomogeneousToEuclidean(const Mat3X &h, Mat2X *e) {
e->resize(2, h.cols());
e->row(0) = h.row(0).array() / h.row(2).array();
e->row(1) = h.row(1).array() / h.row(2).array();
}
Mat2X Project(const Mat34 &P, const Mat3X &X) {
Mat2X x(2, X.cols());
Project(P, X, &x);
return x;
}
void Project(const Mat34 &P, const Mat3X &X, Mat2X *x) {
x->resize(2, X.cols());
for (size_t c = 0; c < static_cast<size_t>(X.cols()); ++c) {
x->col(c) = Project(P, Vec3(X.col(c)));
}
}
ResectionKernel_K(const Mat2X &x_camera, const Mat3X &X) : x_camera_(x_camera), X_(X) {
assert(x_camera.cols() == X.cols());
x_image_ = x_camera_;
K_ = Mat3::Identity();
}