forked from bartvdbraak/blender
libmv: bundle new upstream version from own branch with rigid registration implementation
Currently not used in blender code but is needed for some current work.
This commit is contained in:
parent
ca8fd669a4
commit
e6c45cc1de
2
extern/libmv/CMakeLists.txt
vendored
2
extern/libmv/CMakeLists.txt
vendored
@ -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
|
||||
|
41
extern/libmv/ChangeLog
vendored
41
extern/libmv/ChangeLog
vendored
@ -1,3 +1,36 @@
|
||||
commit fa3842e472e3b9c789e47bf6d8f592aa40a84f16
|
||||
Author: Sergey Sharybin <sergey.vfx@gmail.com>
|
||||
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 <sergey.vfx@gmail.com>
|
||||
Date: Wed Apr 11 14:17:14 2012 +0600
|
||||
@ -493,11 +526,3 @@ Author: Matthias Fauconneau <matthias.fauconneau@gmail.com>
|
||||
Date: Fri Aug 19 14:59:24 2011 +0200
|
||||
|
||||
Expose regularization parameters (areaPenalty and conditionPenalty) in API.
|
||||
|
||||
commit 3e84ae5fbac10451d4935418f6281a90cedace11
|
||||
Author: Matthias Fauconneau <matthias.fauconneau@gmail.com>
|
||||
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).
|
||||
|
2
extern/libmv/files.txt
vendored
2
extern/libmv/files.txt
vendored
@ -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
|
||||
|
56
extern/libmv/libmv-capi.cpp
vendored
56
extern/libmv/libmv-capi.cpp
vendored
@ -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 <stdlib.h>
|
||||
#include <assert.h>
|
||||
@ -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<libmv::Vec3> 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);
|
||||
}
|
||||
|
4
extern/libmv/libmv-capi.h
vendored
4
extern/libmv/libmv-capi.h
vendored
@ -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
|
||||
|
11
extern/libmv/libmv/numeric/numeric.h
vendored
11
extern/libmv/libmv/numeric/numeric.h
vendored
@ -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
|
||||
|
10
extern/libmv/libmv/simple_pipeline/resect.cc
vendored
10
extern/libmv/libmv/simple_pipeline/resect.cc
vendored
@ -42,16 +42,6 @@ Mat2X PointMatrixFromMarkers(const vector<Marker> &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;
|
||||
|
182
extern/libmv/libmv/simple_pipeline/rigid_registration.cc
vendored
Normal file
182
extern/libmv/libmv/simple_pipeline/rigid_registration.cc
vendored
Normal file
@ -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<class RigidTransformation>
|
||||
struct RigidRegistrationCostFunction {
|
||||
public:
|
||||
typedef Vec FMatrixType;
|
||||
typedef RigidTransformation XMatrixType;
|
||||
|
||||
RigidRegistrationCostFunction(const vector<Vec3> &reference_points,
|
||||
const vector<Vec3> &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<Vec3> reference_points_;
|
||||
vector<Vec3> points_;
|
||||
};
|
||||
|
||||
static double RigidRegistrationError(const vector<Vec3> &reference_points,
|
||||
const vector<Vec3> &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<Vec3> &reference_points,
|
||||
const vector<Vec3> &points,
|
||||
Mat3 &R,
|
||||
Vec3 &S,
|
||||
Vec3 &t) {
|
||||
typedef LevenbergMarquardt<RigidRegistrationCostFunction <Vec9> > Solver;
|
||||
|
||||
RigidRegistrationCostFunction<Vec9> 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<Vec3> &reference_points,
|
||||
const vector<Vec3> &points,
|
||||
Mat3 &R,
|
||||
Vec3 &t) {
|
||||
typedef LevenbergMarquardt<RigidRegistrationCostFunction <Vec6> > Solver;
|
||||
|
||||
RigidRegistrationCostFunction<Vec6> 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<Vec3> &reference_points,
|
||||
const vector<Vec3> &points,
|
||||
Mat3 &R) {
|
||||
typedef LevenbergMarquardt<RigidRegistrationCostFunction <Vec3> > Solver;
|
||||
|
||||
RigidRegistrationCostFunction<Vec3> 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
|
68
extern/libmv/libmv/simple_pipeline/rigid_registration.h
vendored
Normal file
68
extern/libmv/libmv/simple_pipeline/rigid_registration.h
vendored
Normal file
@ -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<Vec3> &reference_points,
|
||||
const vector<Vec3> &points,
|
||||
Mat3 &R,
|
||||
Vec3 &S,
|
||||
Vec3 &t);
|
||||
|
||||
/*!
|
||||
* Same as RigidRegistration but provides registration of rotation and translation only
|
||||
*/
|
||||
double RigidRegistration(const vector<Vec3> &reference_points,
|
||||
const vector<Vec3> &points,
|
||||
Mat3 &R,
|
||||
Vec3 &t);
|
||||
|
||||
/*!
|
||||
* Same as RigidRegistration but provides registration of rotation only
|
||||
*/
|
||||
double RigidRegistration(const vector<Vec3> &reference_points,
|
||||
const vector<Vec3> &points,
|
||||
Mat3 &R);
|
||||
|
||||
} // namespace libmv
|
||||
|
||||
#endif // LIBMV_SIMPLE_PIPELINE_RIGID_REGISTRATION_H_
|
Loading…
Reference in New Issue
Block a user