diff --git a/extern/libmv/CMakeLists.txt b/extern/libmv/CMakeLists.txt index 51f254264b6..02723b64b62 100644 --- a/extern/libmv/CMakeLists.txt +++ b/extern/libmv/CMakeLists.txt @@ -72,6 +72,7 @@ set(SRC libmv/simple_pipeline/pipeline.cc libmv/simple_pipeline/reconstruction.cc libmv/simple_pipeline/resect.cc + libmv/simple_pipeline/rigid_registration.cc libmv/simple_pipeline/tracks.cc libmv/tracking/brute_region_tracker.cc libmv/tracking/esm_region_tracker.cc @@ -128,6 +129,7 @@ set(SRC libmv/simple_pipeline/pipeline.h libmv/simple_pipeline/reconstruction.h libmv/simple_pipeline/resect.h + libmv/simple_pipeline/rigid_registration.h libmv/simple_pipeline/tracks.h libmv/tracking/brute_region_tracker.h libmv/tracking/esm_region_tracker.h diff --git a/extern/libmv/ChangeLog b/extern/libmv/ChangeLog index ac5a404bafa..33068bddf90 100644 --- a/extern/libmv/ChangeLog +++ b/extern/libmv/ChangeLog @@ -1,3 +1,36 @@ +commit fa3842e472e3b9c789e47bf6d8f592aa40a84f16 +Author: Sergey Sharybin +Date: Thu Apr 12 12:32:48 2012 +0600 + + implementation of some basic algorithms for point cloud orientation: + + - Implementation of rigid registration algorithm which searches transformation + form one point cloud to another assuming that points in this clouds are + already paired (points with the same index in different clouds belongs to + the same pair) which minimizes average distance between points in pairs. + + Algorithm uses Levenberg-Marquardt solver to find such transformation. + + Supports registration of rotation-scale-transform (which is probably most + common usage) and rotation only (which might be useful for basic modal + tripod solver). + + - Implementation of Iterative-Point-Clouds algorithm which searches + transformation from one arbitrary point cloud to another making + points as closest to each other as possible. + + This algorithm doesn't require points be initially paired, but for + good result clouds should have rough initial orientation. If they're + arbitrary oriented from the very beginning, algorithm might fail + producing good resold. + + Iteration is based on building pairs of closest to each other points + and registering rigid transformation between them which incrementally + constructs final result. + + TODO: building pairs might be speedup a lot using data structures like + AABB trees, K-D trees or so. + commit 9618d9a1d48bb3c28da605d9027f57a74f462785 Author: Sergey Sharybin Date: Wed Apr 11 14:17:14 2012 +0600 @@ -493,11 +526,3 @@ Author: Matthias Fauconneau Date: Fri Aug 19 14:59:24 2011 +0200 Expose regularization parameters (areaPenalty and conditionPenalty) in API. - -commit 3e84ae5fbac10451d4935418f6281a90cedace11 -Author: Matthias Fauconneau -Date: Fri Aug 19 14:19:27 2011 +0200 - - Add LaplaceFilter. - Add regularization in affine SAD Tracker (keep constant area and good condition number). - UI: Better track display (+enable line antialiasing). diff --git a/extern/libmv/files.txt b/extern/libmv/files.txt index 426a573de4e..1e564d3a2f2 100644 --- a/extern/libmv/files.txt +++ b/extern/libmv/files.txt @@ -48,6 +48,8 @@ libmv/simple_pipeline/reconstruction.cc libmv/simple_pipeline/reconstruction.h libmv/simple_pipeline/resect.cc libmv/simple_pipeline/resect.h +libmv/simple_pipeline/rigid_registration.cc +libmv/simple_pipeline/rigid_registration.h libmv/simple_pipeline/tracks.cc libmv/simple_pipeline/tracks.h libmv/tracking/brute_region_tracker.cc diff --git a/extern/libmv/libmv-capi.cpp b/extern/libmv/libmv-capi.cpp index 7fd4bfdd9d1..e4708e5907d 100644 --- a/extern/libmv/libmv-capi.cpp +++ b/extern/libmv/libmv-capi.cpp @@ -36,6 +36,8 @@ #include "Math/v3d_optimization.h" +#include "libmv/numeric/numeric.h" + #include "libmv/tracking/esm_region_tracker.h" #include "libmv/tracking/brute_region_tracker.h" #include "libmv/tracking/hybrid_region_tracker.h" @@ -51,6 +53,7 @@ #include "libmv/simple_pipeline/detect.h" #include "libmv/simple_pipeline/pipeline.h" #include "libmv/simple_pipeline/camera_intrinsics.h" +#include "libmv/simple_pipeline/rigid_registration.h" #include #include @@ -838,3 +841,56 @@ void libmv_InvertIntrinsics(double focal_length, double principal_x, double prin intrinsics.InvertIntrinsics(x, y, x1, y1); } } + +/* ************ point clouds ************ */ + +void libmvTransformToMat4(libmv::Mat3 &R, libmv::Vec3 &S, libmv::Vec3 &t, double M[4][4]) +{ + for (int j = 0; j < 3; ++j) + for (int k = 0; k < 3; ++k) + M[j][k] = R(k, j) * S(j); + + for (int i = 0; i < 3; ++i) { + M[3][0] = t(0); + M[3][1] = t(1); + M[3][2] = t(2); + + M[0][3] = M[1][3] = M[2][3] = 0; + } + + M[3][3] = 1.0; +} + +void libmv_rigidRegistration(float (*reference_points)[3], float (*points)[3], int total_points, + int use_scale, int use_translation, double M[4][4]) +{ + libmv::Mat3 R; + libmv::Vec3 S; + libmv::Vec3 t; + libmv::vector reference_points_vector, points_vector; + + for (int i = 0; i < total_points; i++) { + reference_points_vector.push_back(libmv::Vec3(reference_points[i][0], + reference_points[i][1], + reference_points[i][2])); + + points_vector.push_back(libmv::Vec3(points[i][0], + points[i][1], + points[i][2])); + } + + if (use_scale && use_translation) { + libmv::RigidRegistration(reference_points_vector, points_vector, R, S, t); + } + else if (use_translation) { + S = libmv::Vec3(1.0, 1.0, 1.0); + libmv::RigidRegistration(reference_points_vector, points_vector, R, t); + } + else { + S = libmv::Vec3(1.0, 1.0, 1.0); + t = libmv::Vec3::Zero(); + libmv::RigidRegistration(reference_points_vector, points_vector, R); + } + + libmvTransformToMat4(R, S, t, M); +} diff --git a/extern/libmv/libmv-capi.h b/extern/libmv/libmv-capi.h index e5728188afb..01019832374 100644 --- a/extern/libmv/libmv-capi.h +++ b/extern/libmv/libmv-capi.h @@ -129,6 +129,10 @@ void libmv_applyCameraIntrinsics(double focal_length, double principal_x, double void libmv_InvertIntrinsics(double focal_length, double principal_x, double principal_y, double k1, double k2, double k3, double x, double y, double *x1, double *y1); +/* point clouds */ +void libmv_rigidRegistration(float (*reference_points)[3], float (*points)[3], int total_points, + int use_scale, int use_translation, double M[4][4]); + #ifdef __cplusplus } #endif diff --git a/extern/libmv/libmv/numeric/numeric.h b/extern/libmv/libmv/numeric/numeric.h index bb7f9b364ef..ad707fb76f2 100644 --- a/extern/libmv/libmv/numeric/numeric.h +++ b/extern/libmv/libmv/numeric/numeric.h @@ -474,6 +474,17 @@ inline Mat23 SkewMatMinimal(const Vec2 &x) { 1, 0, -x(0); return skew; } + +/// Returns the rotaiton matrix built from given vector of euler angles +inline Mat3 RotationFromEulerVector(Vec3 euler_vector) { + double theta = euler_vector.norm(); + if (theta == 0.0) { + return Mat3::Identity(); + } + Vec3 w = euler_vector / theta; + Mat3 w_hat = CrossProductMatrix(w); + return Mat3::Identity() + w_hat*sin(theta) + w_hat*w_hat*(1 - cos(theta)); +} } // namespace libmv #endif // LIBMV_NUMERIC_NUMERIC_H diff --git a/extern/libmv/libmv/simple_pipeline/resect.cc b/extern/libmv/libmv/simple_pipeline/resect.cc index 6e71c3c7206..b30d959b512 100644 --- a/extern/libmv/libmv/simple_pipeline/resect.cc +++ b/extern/libmv/libmv/simple_pipeline/resect.cc @@ -42,16 +42,6 @@ Mat2X PointMatrixFromMarkers(const vector &markers) { return points; } -Mat3 RotationFromEulerVector(Vec3 euler_vector) { - double theta = euler_vector.norm(); - if (theta == 0.0) { - return Mat3::Identity(); - } - Vec3 w = euler_vector / theta; - Mat3 w_hat = CrossProductMatrix(w); - return Mat3::Identity() + w_hat*sin(theta) + w_hat*w_hat*(1 - cos(theta)); -} - // Uses an incremental rotation: // // x = R' * R * X + t; diff --git a/extern/libmv/libmv/simple_pipeline/rigid_registration.cc b/extern/libmv/libmv/simple_pipeline/rigid_registration.cc new file mode 100644 index 00000000000..f7cb1e66e7d --- /dev/null +++ b/extern/libmv/libmv/simple_pipeline/rigid_registration.cc @@ -0,0 +1,182 @@ +// Copyright (c) 2012 libmv authors. +// +// Permission is hereby granted, free of charge, to any person obtaining a copy +// of this software and associated documentation files (the "Software"), to +// deal in the Software without restriction, including without limitation the +// rights to use, copy, modify, merge, publish, distribute, sublicense, and/or +// sell copies of the Software, and to permit persons to whom the Software is +// furnished to do so, subject to the following conditions: +// +// The above copyright notice and this permission notice shall be included in +// all copies or substantial portions of the Software. +// +// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR +// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, +// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE +// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER +// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING +// FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS +// IN THE SOFTWARE. + +#include "libmv/simple_pipeline/rigid_registration.h" +#include "libmv/numeric/levenberg_marquardt.h" + +namespace libmv { + +template +struct RigidRegistrationCostFunction { + public: + typedef Vec FMatrixType; + typedef RigidTransformation XMatrixType; + + RigidRegistrationCostFunction(const vector &reference_points, + const vector &points): + reference_points_(reference_points), + points_(points) {} + + Vec CalculateResiduals(const Mat3 &R, + const Vec3 &S, + const Vec3 &t) const { + Vec residuals(points_.size()); + residuals.setZero(); + + // Convert scale vector to matrix + Mat3 SMat = Mat3::Identity(); + SMat(0, 0) *= S(0); + SMat(1, 1) *= S(1); + SMat(2, 2) *= S(2); + + for (int i = 0; i < points_.size(); i++) { + Vec3 transformed_point = R * SMat * points_[i] + t; + + double norm = (transformed_point - reference_points_[i]).norm(); + + residuals(i) = norm * norm; + } + + return residuals; + } + + Vec operator()(const Vec9 &RSt) const { + Mat3 R = RotationFromEulerVector(RSt.head<3>()); + Vec3 S = RSt.segment<3>(3); + Vec3 t = RSt.tail<3>(); + + return CalculateResiduals(R, S, t); + } + + Vec operator()(const Vec3 &euler) const { + Mat3 R = RotationFromEulerVector(euler); + + return CalculateResiduals(R, Vec3(1.0, 1.0, 1.0), Vec3::Zero()); + } + + Vec operator()(const Vec6 &Rt) const { + Mat3 R = RotationFromEulerVector(Rt.head<3>()); + Vec3 t = Rt.tail<3>(); + + return CalculateResiduals(R, Vec3(1.0, 1.0, 1.0), t); + } + + private: + vector reference_points_; + vector points_; +}; + +static double RigidRegistrationError(const vector &reference_points, + const vector &points, + const Mat3 &R, + const Vec3 &S, + const Vec3 &t) { + double error = 0.0; + + Mat3 SMat = Mat3::Identity(); + SMat(0, 0) *= S(0); + SMat(1, 1) *= S(1); + SMat(2, 2) *= S(2); + + for (int i = 0; i < points.size(); i++) { + Vec3 new_point = R * SMat * points[i] + t; + + double norm = (new_point - reference_points[i]).norm(); + error += norm * norm; + } + error /= points.size(); + + return error; +} + +double RigidRegistration(const vector &reference_points, + const vector &points, + Mat3 &R, + Vec3 &S, + Vec3 &t) { + typedef LevenbergMarquardt > Solver; + + RigidRegistrationCostFunction rigidregistration_cost(reference_points, points); + Solver solver(rigidregistration_cost); + + Vec9 RSt = Vec9::Zero(); + + RSt(3) = RSt(4) = RSt(5) = 1.0; + + Solver::SolverParameters params; + /*Solver::Results results = */ solver.minimize(params, &RSt); + /* TODO(sergey): better error handling here */ + + LG << "Rigid registration completed, rotation is:" << RSt.head<3>().transpose() + << ", scale is " << RSt.segment<3>(3).transpose() + << ", translation is " << RSt.tail<3>().transpose(); + + // Decompose individual rotation and translation + R = RotationFromEulerVector(RSt.head<3>()); + S = RSt.segment<3>(3); + t = RSt.tail<3>(); + + return RigidRegistrationError(reference_points, points, R, S, t); +} + +double RigidRegistration(const vector &reference_points, + const vector &points, + Mat3 &R, + Vec3 &t) { + typedef LevenbergMarquardt > Solver; + + RigidRegistrationCostFunction rigidregistration_cost(reference_points, points); + Solver solver(rigidregistration_cost); + + Vec6 Rt = Vec6::Zero(); + Solver::SolverParameters params; + /*Solver::Results results = */solver.minimize(params, &Rt); + /* TODO(sergey): better error handling here */ + + LG << "Rigid registration completed, rotation is:" << Rt.head<3>().transpose() + << ", translation is " << Rt.tail<3>().transpose(); + + R = RotationFromEulerVector(Rt.head<3>()); + t = Rt.tail<3>(); + + return RigidRegistrationError(reference_points, points, R, Vec3(1.0, 1.0, 1.0), t); +} + +double RigidRegistration(const vector &reference_points, + const vector &points, + Mat3 &R) { + typedef LevenbergMarquardt > Solver; + + RigidRegistrationCostFunction rigidregistration_cost(reference_points, points); + Solver solver(rigidregistration_cost); + + Vec3 euler = Vec3::Zero(); + Solver::SolverParameters params; + /*Solver::Results results = */solver.minimize(params, &euler); + /* TODO(sergey): better error handling here */ + + LG << "Rigid registration completed, rotation is:" << euler.transpose(); + + R = RotationFromEulerVector(euler); + + return RigidRegistrationError(reference_points, points, R, Vec3(1.0, 1.0, 1.0), Vec3::Zero()); +} + +} // namespace libmv diff --git a/extern/libmv/libmv/simple_pipeline/rigid_registration.h b/extern/libmv/libmv/simple_pipeline/rigid_registration.h new file mode 100644 index 00000000000..21ea57af507 --- /dev/null +++ b/extern/libmv/libmv/simple_pipeline/rigid_registration.h @@ -0,0 +1,68 @@ +// Copyright (c) 2012 libmv authors. +// +// Permission is hereby granted, free of charge, to any person obtaining a copy +// of this software and associated documentation files (the "Software"), to +// deal in the Software without restriction, including without limitation the +// rights to use, copy, modify, merge, publish, distribute, sublicense, and/or +// sell copies of the Software, and to permit persons to whom the Software is +// furnished to do so, subject to the following conditions: +// +// The above copyright notice and this permission notice shall be included in +// all copies or substantial portions of the Software. +// +// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR +// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, +// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE +// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER +// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING +// FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS +// IN THE SOFTWARE. + +#ifndef LIBMV_SIMPLE_PIPELINE_RIGID_REGISTRATION_H_ +#define LIBMV_SIMPLE_PIPELINE_RIGID_REGISTRATION_H_ + +#include "libmv/base/vector.h" +#include "libmv/numeric/numeric.h" + +namespace libmv { + +/*! + Searched for an affine transformation of rigid 3D object defined by it's + vertices positions from it's current state called points to it's desired + state called reference points. + + Returns rotation matrix, per-component scale vector and translation which + transforms points to the mot close state to reference_points. + + Return value is an average squared distance between reference state + and transformed one. + + Minimzation of distance between point pairs is used to register such a + rigid transformation and algorithm assumes that pairs of points are + defined by point's index in a vector, so points with the same index + belongs to the same pair. + */ +double RigidRegistration(const vector &reference_points, + const vector &points, + Mat3 &R, + Vec3 &S, + Vec3 &t); + +/*! + * Same as RigidRegistration but provides registration of rotation and translation only + */ +double RigidRegistration(const vector &reference_points, + const vector &points, + Mat3 &R, + Vec3 &t); + +/*! + * Same as RigidRegistration but provides registration of rotation only + */ +double RigidRegistration(const vector &reference_points, + const vector &points, + Mat3 &R); + +} // namespace libmv + +#endif // LIBMV_SIMPLE_PIPELINE_RIGID_REGISTRATION_H_