23 #include "ceres/ceres.h"
47 const Mat& x1,
const Mat&
x2,
Mat3*
H,
double expected_precision) {
48 assert(2 == x1.rows());
49 assert(4 <= x1.cols());
50 assert(x1.rows() ==
x2.rows());
51 assert(x1.cols() ==
x2.cols());
54 MatX8 L = Mat::Zero(n * 3, 8);
55 Mat b = Mat::Zero(n * 3, 1);
56 for (
int i = 0; i < n; ++i) {
61 L(j, 6) = -
x2(0, i) * x1(0, i);
62 L(j, 7) = -
x2(0, i) * x1(1, i);
69 L(j, 6) = -
x2(1, i) * x1(0, i);
70 L(j, 7) = -
x2(1, i) * x1(1, i);
76 L(j, 0) =
x2(1, i) * x1(0, i);
77 L(j, 1) =
x2(1, i) * x1(1, i);
79 L(j, 3) = -
x2(0, i) * x1(0, i);
80 L(j, 4) = -
x2(0, i) * x1(1, i);
84 Vec h =
L.fullPivLu().solve(
b);
86 if ((
L * h).isApprox(
b, expected_precision)) {
106 double expected_precision) {
107 if (x1.rows() == 2) {
109 x1,
x2,
H, expected_precision);
111 assert(3 == x1.rows());
112 assert(4 <= x1.cols());
113 assert(x1.rows() ==
x2.rows());
114 assert(x1.cols() ==
x2.cols());
120 MatX8 L = Mat::Zero(n * 3, 8);
121 Mat b = Mat::Zero(n * 3, 1);
122 for (
int i = 0; i < n; ++i) {
124 L(j, 0) =
x2(
w, i) * x1(
x, i);
125 L(j, 1) =
x2(
w, i) * x1(
y, i);
126 L(j, 2) =
x2(
w, i) * x1(
w, i);
127 L(j, 6) = -
x2(
x, i) * x1(
x, i);
128 L(j, 7) = -
x2(
x, i) * x1(
y, i);
129 b(j, 0) =
x2(
x, i) * x1(
w, i);
132 L(j, 3) =
x2(
w, i) * x1(
x, i);
133 L(j, 4) =
x2(
w, i) * x1(
y, i);
134 L(j, 5) =
x2(
w, i) * x1(
w, i);
135 L(j, 6) = -
x2(
y, i) * x1(
x, i);
136 L(j, 7) = -
x2(
y, i) * x1(
y, i);
137 b(j, 0) =
x2(
y, i) * x1(
w, i);
141 L(j, 0) =
x2(
y, i) * x1(
x, i);
142 L(j, 1) =
x2(
y, i) * x1(
y, i);
143 L(j, 2) =
x2(
y, i) * x1(
w, i);
144 L(j, 3) = -
x2(
x, i) * x1(
x, i);
145 L(j, 4) = -
x2(
x, i) * x1(
y, i);
146 L(j, 5) = -
x2(
x, i) * x1(
w, i);
149 Vec h =
L.fullPivLu().solve(
b);
150 if ((
L * h).isApprox(
b, expected_precision)) {
161 : use_normalization(true),
162 max_num_iterations(50),
163 expected_average_symmetric_distance(1
e-16) {
167 void GetNormalizedPoints(
const Mat& original_points,
168 Mat* normalized_points,
169 Mat3* normalization_matrix) {
172 original_points, *normalization_matrix, normalized_points);
177 class HomographySymmetricGeometricCostFunctor {
179 HomographySymmetricGeometricCostFunctor(
const Vec2&
x,
const Vec2&
y)
182 template <
typename T>
183 bool operator()(
const T* homography_parameters,
T* residuals)
const {
184 typedef Eigen::Matrix<T, 3, 3>
Mat3;
185 typedef Eigen::Matrix<T, 3, 1>
Vec3;
187 Mat3 H(homography_parameters);
193 Vec3 Hinv_y =
H.inverse() *
y;
199 residuals[0] = H_x(0) -
T(
y_(0));
200 residuals[1] = H_x(1) -
T(
y_(1));
203 residuals[2] = Hinv_y(0) -
T(
x_(0));
204 residuals[3] = Hinv_y(1) -
T(
x_(1));
209 EIGEN_MAKE_ALIGNED_OPERATOR_NEW
219 class TerminationCheckingCallback :
public ceres::IterationCallback {
221 TerminationCheckingCallback(
const Mat& x1,
223 const EstimateHomographyOptions&
options,
225 : options_(
options), x1_(x1), x2_(
x2), H_(
H) {}
228 const ceres::IterationSummary& summary) {
230 if (!summary.step_is_successful) {
231 return ceres::SOLVER_CONTINUE;
235 double average_distance = 0.0;
236 for (
int i = 0; i < x1_.cols(); i++) {
240 average_distance /= x1_.cols();
242 if (average_distance <= options_.expected_average_symmetric_distance) {
243 return ceres::SOLVER_TERMINATE_SUCCESSFULLY;
246 return ceres::SOLVER_CONTINUE;
250 const EstimateHomographyOptions& options_;
267 assert(2 == x1.rows());
268 assert(4 <= x1.cols());
269 assert(x1.rows() ==
x2.rows());
270 assert(x1.cols() ==
x2.cols());
272 Mat3 T1 = Mat3::Identity(),
T2 = Mat3::Identity();
275 Mat x1_normalized, x2_normalized;
277 if (
options.use_normalization) {
278 LG <<
"Estimating homography using normalization.";
279 GetNormalizedPoints(x1, &x1_normalized, &
T1);
280 GetNormalizedPoints(
x2, &x2_normalized, &
T2);
290 if (
options.use_normalization) {
291 *
H =
T2.inverse() * (*H) *
T1;
294 LG <<
"Estimated matrix after algebraic estimation:\n" << *
H;
297 ceres::Problem problem;
298 for (
int i = 0; i < x1.cols(); i++) {
299 HomographySymmetricGeometricCostFunctor*
300 homography_symmetric_geometric_cost_function =
301 new HomographySymmetricGeometricCostFunctor(x1.col(i),
x2.col(i));
303 problem.AddResidualBlock(
304 new ceres::AutoDiffCostFunction<HomographySymmetricGeometricCostFunctor,
307 homography_symmetric_geometric_cost_function),
313 ceres::Solver::Options solver_options;
314 solver_options.linear_solver_type = ceres::DENSE_QR;
315 solver_options.max_num_iterations =
options.max_num_iterations;
316 solver_options.update_state_every_iteration =
true;
320 solver_options.callbacks.push_back(&
callback);
323 ceres::Solver::Summary summary;
324 ceres::Solve(solver_options, &problem, &summary);
326 VLOG(1) <<
"Summary:\n" << summary.FullReport();
328 LG <<
"Final refined matrix:\n" << *
H;
330 return summary.IsSolutionUsable();
365 double expected_precision) {
366 assert(4 == x1.rows());
367 assert(5 <= x1.cols());
368 assert(x1.rows() ==
x2.rows());
369 assert(x1.cols() ==
x2.cols());
375 MatX15 L = Mat::Zero(n * 6, 15);
376 Mat b = Mat::Zero(n * 6, 1);
377 for (
int i = 0; i < n; ++i) {
379 L(j, 0) = -
x2(
w, i) * x1(
x, i);
380 L(j, 1) = -
x2(
w, i) * x1(
y, i);
381 L(j, 2) = -
x2(
w, i) * x1(
z, i);
382 L(j, 3) = -
x2(
w, i) * x1(
w, i);
383 L(j, 12) =
x2(
x, i) * x1(
x, i);
384 L(j, 13) =
x2(
x, i) * x1(
y, i);
385 L(j, 14) =
x2(
x, i) * x1(
z, i);
386 b(j, 0) = -
x2(
x, i) * x1(
w, i);
389 L(j, 4) = -
x2(
z, i) * x1(
x, i);
390 L(j, 5) = -
x2(
z, i) * x1(
y, i);
391 L(j, 6) = -
x2(
z, i) * x1(
z, i);
392 L(j, 7) = -
x2(
z, i) * x1(
w, i);
393 L(j, 8) =
x2(
y, i) * x1(
x, i);
394 L(j, 9) =
x2(
y, i) * x1(
y, i);
395 L(j, 10) =
x2(
y, i) * x1(
z, i);
396 L(j, 11) =
x2(
y, i) * x1(
w, i);
399 L(j, 0) = -
x2(
z, i) * x1(
x, i);
400 L(j, 1) = -
x2(
z, i) * x1(
y, i);
401 L(j, 2) = -
x2(
z, i) * x1(
z, i);
402 L(j, 3) = -
x2(
z, i) * x1(
w, i);
403 L(j, 8) =
x2(
x, i) * x1(
x, i);
404 L(j, 9) =
x2(
x, i) * x1(
y, i);
405 L(j, 10) =
x2(
x, i) * x1(
z, i);
406 L(j, 11) =
x2(
x, i) * x1(
w, i);
409 L(j, 4) = -
x2(
w, i) * x1(
x, i);
410 L(j, 5) = -
x2(
w, i) * x1(
y, i);
411 L(j, 6) = -
x2(
w, i) * x1(
z, i);
412 L(j, 7) = -
x2(
w, i) * x1(
w, i);
413 L(j, 12) =
x2(
y, i) * x1(
x, i);
414 L(j, 13) =
x2(
y, i) * x1(
y, i);
415 L(j, 14) =
x2(
y, i) * x1(
z, i);
416 b(j, 0) = -
x2(
y, i) * x1(
w, i);
419 L(j, 0) = -
x2(
y, i) * x1(
x, i);
420 L(j, 1) = -
x2(
y, i) * x1(
y, i);
421 L(j, 2) = -
x2(
y, i) * x1(
z, i);
422 L(j, 3) = -
x2(
y, i) * x1(
w, i);
423 L(j, 4) =
x2(
x, i) * x1(
x, i);
424 L(j, 5) =
x2(
x, i) * x1(
y, i);
425 L(j, 6) =
x2(
x, i) * x1(
z, i);
426 L(j, 7) =
x2(
x, i) * x1(
w, i);
429 L(j, 8) = -
x2(
w, i) * x1(
x, i);
430 L(j, 9) = -
x2(
w, i) * x1(
y, i);
431 L(j, 10) = -
x2(
w, i) * x1(
z, i);
432 L(j, 11) = -
x2(
w, i) * x1(
w, i);
433 L(j, 12) =
x2(
z, i) * x1(
x, i);
434 L(j, 13) =
x2(
z, i) * x1(
y, i);
435 L(j, 14) =
x2(
z, i) * x1(
z, i);
436 b(j, 0) = -
x2(
z, i) * x1(
w, i);
439 Vec h =
L.fullPivLu().solve(
b);
440 if ((
L * h).isApprox(
b, expected_precision)) {
451 Vec3 x(x1(0), x1(1), 1.0);
455 Vec3 Hinv_y =
H.inverse() *
y;
460 return (H_x.head<2>() -
y.head<2>()).squaredNorm() +
461 (Hinv_y.head<2>() -
x.head<2>()).squaredNorm();
_GL_VOID GLfloat value _GL_VOID_RET _GL_VOID const GLuint GLboolean *residences _GL_BOOL_RET _GL_VOID GLsizei GLfloat GLfloat GLfloat GLfloat const GLubyte *bitmap _GL_VOID_RET _GL_VOID GLenum const void *lists _GL_VOID_RET _GL_VOID const GLdouble *equation _GL_VOID_RET _GL_VOID GLdouble GLdouble blue _GL_VOID_RET _GL_VOID GLfloat GLfloat blue _GL_VOID_RET _GL_VOID GLint GLint blue _GL_VOID_RET _GL_VOID GLshort GLshort blue _GL_VOID_RET _GL_VOID GLubyte GLubyte blue _GL_VOID_RET _GL_VOID GLuint GLuint blue _GL_VOID_RET _GL_VOID GLushort GLushort blue _GL_VOID_RET _GL_VOID GLbyte GLbyte GLbyte alpha _GL_VOID_RET _GL_VOID GLdouble GLdouble GLdouble alpha _GL_VOID_RET _GL_VOID GLfloat GLfloat GLfloat alpha _GL_VOID_RET _GL_VOID GLint GLint GLint alpha _GL_VOID_RET _GL_VOID GLshort GLshort GLshort alpha _GL_VOID_RET _GL_VOID GLubyte GLubyte GLubyte alpha _GL_VOID_RET _GL_VOID GLuint GLuint GLuint alpha _GL_VOID_RET _GL_VOID GLushort GLushort GLushort alpha _GL_VOID_RET _GL_VOID GLenum mode _GL_VOID_RET _GL_VOID GLint GLsizei GLsizei GLenum type _GL_VOID_RET _GL_VOID GLsizei GLenum GLenum const void *pixels _GL_VOID_RET _GL_VOID const void *pointer _GL_VOID_RET _GL_VOID GLdouble v _GL_VOID_RET _GL_VOID GLfloat v _GL_VOID_RET _GL_VOID GLint GLint i2 _GL_VOID_RET _GL_VOID GLint j _GL_VOID_RET _GL_VOID GLfloat param _GL_VOID_RET _GL_VOID GLint param _GL_VOID_RET _GL_VOID GLdouble GLdouble GLdouble GLdouble GLdouble zFar _GL_VOID_RET _GL_UINT GLdouble *equation _GL_VOID_RET _GL_VOID GLenum GLint *params _GL_VOID_RET _GL_VOID GLenum GLfloat *v _GL_VOID_RET _GL_VOID GLenum GLfloat *params _GL_VOID_RET _GL_VOID GLfloat *values _GL_VOID_RET _GL_VOID GLushort *values _GL_VOID_RET _GL_VOID GLenum GLfloat *params _GL_VOID_RET _GL_VOID GLenum GLdouble *params _GL_VOID_RET _GL_VOID GLenum GLint *params _GL_VOID_RET _GL_VOID GLsizei const void *pointer _GL_VOID_RET _GL_VOID GLsizei const void *pointer _GL_VOID_RET _GL_BOOL GLfloat param _GL_VOID_RET _GL_VOID GLint param _GL_VOID_RET _GL_VOID GLenum GLfloat param _GL_VOID_RET _GL_VOID GLenum GLint param _GL_VOID_RET _GL_VOID GLushort pattern _GL_VOID_RET _GL_VOID GLdouble GLdouble GLint GLint const GLdouble *points _GL_VOID_RET _GL_VOID GLdouble GLdouble GLint GLint GLdouble GLdouble GLint GLint const GLdouble *points _GL_VOID_RET _GL_VOID GLdouble GLdouble u2 _GL_VOID_RET _GL_VOID GLdouble GLdouble GLint GLdouble GLdouble v2 _GL_VOID_RET _GL_VOID GLenum GLfloat param _GL_VOID_RET _GL_VOID GLenum GLint param _GL_VOID_RET _GL_VOID GLenum mode _GL_VOID_RET _GL_VOID GLdouble GLdouble nz _GL_VOID_RET _GL_VOID GLfloat GLfloat nz _GL_VOID_RET _GL_VOID GLint GLint nz _GL_VOID_RET _GL_VOID GLshort GLshort nz _GL_VOID_RET _GL_VOID GLsizei const void *pointer _GL_VOID_RET _GL_VOID GLsizei const GLfloat *values _GL_VOID_RET _GL_VOID GLsizei const GLushort *values _GL_VOID_RET _GL_VOID GLint param _GL_VOID_RET _GL_VOID const GLuint const GLclampf *priorities _GL_VOID_RET _GL_VOID GLdouble y _GL_VOID_RET _GL_VOID GLfloat y _GL_VOID_RET _GL_VOID GLint y _GL_VOID_RET _GL_VOID GLshort y _GL_VOID_RET _GL_VOID GLdouble GLdouble z _GL_VOID_RET _GL_VOID GLfloat GLfloat z _GL_VOID_RET _GL_VOID GLint GLint z _GL_VOID_RET _GL_VOID GLshort GLshort z _GL_VOID_RET _GL_VOID GLdouble GLdouble z
_GL_VOID GLfloat value _GL_VOID_RET _GL_VOID const GLuint GLboolean *residences _GL_BOOL_RET _GL_VOID GLsizei GLfloat GLfloat GLfloat GLfloat const GLubyte *bitmap _GL_VOID_RET _GL_VOID GLenum const void *lists _GL_VOID_RET _GL_VOID const GLdouble *equation _GL_VOID_RET _GL_VOID GLdouble GLdouble blue _GL_VOID_RET _GL_VOID GLfloat GLfloat blue _GL_VOID_RET _GL_VOID GLint GLint blue _GL_VOID_RET _GL_VOID GLshort GLshort blue _GL_VOID_RET _GL_VOID GLubyte GLubyte blue _GL_VOID_RET _GL_VOID GLuint GLuint blue _GL_VOID_RET _GL_VOID GLushort GLushort blue _GL_VOID_RET _GL_VOID GLbyte GLbyte GLbyte alpha _GL_VOID_RET _GL_VOID GLdouble GLdouble GLdouble alpha _GL_VOID_RET _GL_VOID GLfloat GLfloat GLfloat alpha _GL_VOID_RET _GL_VOID GLint GLint GLint alpha _GL_VOID_RET _GL_VOID GLshort GLshort GLshort alpha _GL_VOID_RET _GL_VOID GLubyte GLubyte GLubyte alpha _GL_VOID_RET _GL_VOID GLuint GLuint GLuint alpha _GL_VOID_RET _GL_VOID GLushort GLushort GLushort alpha _GL_VOID_RET _GL_VOID GLenum mode _GL_VOID_RET _GL_VOID GLint y
_GL_VOID GLfloat value _GL_VOID_RET _GL_VOID const GLuint GLboolean *residences _GL_BOOL_RET _GL_VOID GLsizei GLfloat GLfloat GLfloat GLfloat const GLubyte *bitmap _GL_VOID_RET _GL_VOID GLenum const void *lists _GL_VOID_RET _GL_VOID const GLdouble *equation _GL_VOID_RET _GL_VOID GLdouble GLdouble blue _GL_VOID_RET _GL_VOID GLfloat GLfloat blue _GL_VOID_RET _GL_VOID GLint GLint blue _GL_VOID_RET _GL_VOID GLshort GLshort blue _GL_VOID_RET _GL_VOID GLubyte GLubyte blue _GL_VOID_RET _GL_VOID GLuint GLuint blue _GL_VOID_RET _GL_VOID GLushort GLushort blue _GL_VOID_RET _GL_VOID GLbyte GLbyte GLbyte alpha _GL_VOID_RET _GL_VOID GLdouble GLdouble GLdouble alpha _GL_VOID_RET _GL_VOID GLfloat GLfloat GLfloat alpha _GL_VOID_RET _GL_VOID GLint GLint GLint alpha _GL_VOID_RET _GL_VOID GLshort GLshort GLshort alpha _GL_VOID_RET _GL_VOID GLubyte GLubyte GLubyte alpha _GL_VOID_RET _GL_VOID GLuint GLuint GLuint alpha _GL_VOID_RET _GL_VOID GLushort GLushort GLushort alpha _GL_VOID_RET _GL_VOID GLenum mode _GL_VOID_RET _GL_VOID GLint GLsizei GLsizei GLenum type _GL_VOID_RET _GL_VOID GLsizei GLenum GLenum const void *pixels _GL_VOID_RET _GL_VOID const void *pointer _GL_VOID_RET _GL_VOID GLdouble v _GL_VOID_RET _GL_VOID GLfloat v _GL_VOID_RET _GL_VOID GLint GLint i2 _GL_VOID_RET _GL_VOID GLint j _GL_VOID_RET _GL_VOID GLfloat param _GL_VOID_RET _GL_VOID GLint param _GL_VOID_RET _GL_VOID GLdouble GLdouble GLdouble GLdouble GLdouble zFar _GL_VOID_RET _GL_UINT GLdouble *equation _GL_VOID_RET _GL_VOID GLenum GLint *params _GL_VOID_RET _GL_VOID GLenum GLfloat *v _GL_VOID_RET _GL_VOID GLenum GLfloat *params _GL_VOID_RET _GL_VOID GLfloat *values _GL_VOID_RET _GL_VOID GLushort *values _GL_VOID_RET _GL_VOID GLenum GLfloat *params _GL_VOID_RET _GL_VOID GLenum GLdouble *params _GL_VOID_RET _GL_VOID GLenum GLint *params _GL_VOID_RET _GL_VOID GLsizei const void *pointer _GL_VOID_RET _GL_VOID GLsizei const void *pointer _GL_VOID_RET _GL_BOOL GLfloat param _GL_VOID_RET _GL_VOID GLint param _GL_VOID_RET _GL_VOID GLenum GLfloat param _GL_VOID_RET _GL_VOID GLenum GLint param _GL_VOID_RET _GL_VOID GLushort pattern _GL_VOID_RET _GL_VOID GLdouble GLdouble GLint GLint const GLdouble *points _GL_VOID_RET _GL_VOID GLdouble GLdouble GLint GLint GLdouble GLdouble GLint GLint const GLdouble *points _GL_VOID_RET _GL_VOID GLdouble GLdouble u2 _GL_VOID_RET _GL_VOID GLdouble GLdouble GLint GLdouble GLdouble v2 _GL_VOID_RET _GL_VOID GLenum GLfloat param _GL_VOID_RET _GL_VOID GLenum GLint param _GL_VOID_RET _GL_VOID GLenum mode _GL_VOID_RET _GL_VOID GLdouble GLdouble nz _GL_VOID_RET _GL_VOID GLfloat GLfloat nz _GL_VOID_RET _GL_VOID GLint GLint nz _GL_VOID_RET _GL_VOID GLshort GLshort nz _GL_VOID_RET _GL_VOID GLsizei const void *pointer _GL_VOID_RET _GL_VOID GLsizei const GLfloat *values _GL_VOID_RET _GL_VOID GLsizei const GLushort *values _GL_VOID_RET _GL_VOID GLint param _GL_VOID_RET _GL_VOID const GLuint const GLclampf *priorities _GL_VOID_RET _GL_VOID GLdouble y _GL_VOID_RET _GL_VOID GLfloat y _GL_VOID_RET _GL_VOID GLint y _GL_VOID_RET _GL_VOID GLshort y _GL_VOID_RET _GL_VOID GLdouble GLdouble z _GL_VOID_RET _GL_VOID GLfloat GLfloat z _GL_VOID_RET _GL_VOID GLint GLint z _GL_VOID_RET _GL_VOID GLshort GLshort z _GL_VOID_RET _GL_VOID GLdouble GLdouble GLdouble w _GL_VOID_RET _GL_VOID GLfloat GLfloat GLfloat w _GL_VOID_RET _GL_VOID GLint GLint GLint w _GL_VOID_RET _GL_VOID GLshort GLshort GLshort w _GL_VOID_RET _GL_VOID GLdouble GLdouble x2
ATTR_WARN_UNUSED_RESULT const BMVert const BMEdge * e
SIMD_FORCE_INLINE const btScalar & w() const
Return the w value.
static void To(const Parameters &p, Parameterized *h)
Convert from the 8 parameters to a H matrix.
static void To(const Parameters &p, Parameterized *h)
Convert from the 15 parameters to a H matrix.
CCL_NAMESPACE_BEGIN struct Options options
DEGForeachIDComponentCallback callback
EIGEN_MAKE_ALIGNED_OPERATOR_NEW const Vec2 x_
Eigen::Matrix< double, Eigen::Dynamic, 15 > MatX15
Eigen::Matrix< double, 3, 3 > Mat3
Eigen::Matrix< double, Eigen::Dynamic, 8 > MatX8
void ApplyTransformationToPoints(const Mat &points, const Mat3 &T, Mat *transformed_points)
bool Homography2DFromCorrespondencesLinear(const Mat &x1, const Mat &x2, Mat3 *H, double expected_precision)
bool EstimateHomography2DFromCorrespondences(const Mat &x1, const Mat &x2, const EstimateHomographyOptions &options, Mat3 *H)
bool Homography3DFromCorrespondencesLinear(const Mat &x1, const Mat &x2, Mat4 *H, double expected_precision)
double SymmetricGeometricDistance(const Mat3 &H, const Vec2 &x1, const Vec2 &x2)
static bool Homography2DFromCorrespondencesLinearEuc(const Mat &x1, const Mat &x2, Mat3 *H, double expected_precision)
void IsotropicPreconditionerFromPoints(const Mat &points, Mat3 *T)
static const pxr::TfToken b("b", pxr::TfToken::Immortal)
EstimateHomographyOptions(void)