// Copyright (C) 2003 Davis E. King (davis@dlib.net)
// License: Boost Software License See LICENSE.txt for the full license.
#ifndef DLIB_POINT_TrANSFORMS_H_
#define DLIB_POINT_TrANSFORMS_H_
#include "point_transforms_abstract.h"
#include "../algs.h"
#include "vector.h"
#include "../matrix.h"
#include "../matrix/matrix_la.h"
#include "../optimization/optimization.h"
#include "rectangle.h"
#include "drectangle.h"
#include <vector>
#include <cmath>
namespace dlib
{
// ----------------------------------------------------------------------------------------
class point_rotator
{
public:
point_rotator (
)
{
sin_angle = 0;
cos_angle = 1;
}
point_rotator (
const double& angle
)
{
sin_angle = std::sin(angle);
cos_angle = std::cos(angle);
}
template <typename T>
const dlib::vector<T,2> operator() (
const dlib::vector<T,2>& p
) const
{
double x = cos_angle*p.x() - sin_angle*p.y();
double y = sin_angle*p.x() + cos_angle*p.y();
return dlib::vector<double,2>(x,y);
}
const matrix<double,2,2> get_m(
) const
{
matrix<double,2,2> temp;
temp = cos_angle, -sin_angle,
sin_angle, cos_angle;
return temp;
}
inline friend void serialize (const point_rotator& item, std::ostream& out)
{
serialize(item.sin_angle, out);
serialize(item.cos_angle, out);
}
inline friend void deserialize (point_rotator& item, std::istream& in)
{
deserialize(item.sin_angle, in);
deserialize(item.cos_angle, in);
}
private:
double sin_angle;
double cos_angle;
};
// ----------------------------------------------------------------------------------------
class point_transform
{
public:
point_transform (
)
{
sin_angle = 0;
cos_angle = 1;
translate.x() = 0;
translate.y() = 0;
}
point_transform (
const double& angle,
const dlib::vector<double,2>& translate_
)
{
sin_angle = std::sin(angle);
cos_angle = std::cos(angle);
translate = translate_;
}
template <typename T>
const dlib::vector<T,2> operator() (
const dlib::vector<T,2>& p
) const
{
double x = cos_angle*p.x() - sin_angle*p.y();
double y = sin_angle*p.x() + cos_angle*p.y();
return dlib::vector<double,2>(x,y) + translate;
}
const matrix<double,2,2> get_m(
) const
{
matrix<double,2,2> temp;
temp = cos_angle, -sin_angle,
sin_angle, cos_angle;
return temp;
}
const dlib::vector<double,2> get_b(
) const { return translate; }
inline friend void serialize (const point_transform& item, std::ostream& out)
{
serialize(item.sin_angle, out);
serialize(item.cos_angle, out);
serialize(item.translate, out);
}
inline friend void deserialize (point_transform& item, std::istream& in)
{
deserialize(item.sin_angle, in);
deserialize(item.cos_angle, in);
deserialize(item.translate, in);
}
private:
double sin_angle;
double cos_angle;
dlib::vector<double,2> translate;
};
// ----------------------------------------------------------------------------------------
class point_transform_affine
{
public:
point_transform_affine (
)
{
m = identity_matrix<double>(2);
b.x() = 0;
b.y() = 0;
}
point_transform_affine (
const matrix<double,2,2>& m_,
const dlib::vector<double,2>& b_
) :m(m_), b(b_)
{
}
const dlib::vector<double,2> operator() (
const dlib::vector<double,2>& p
) const
{
return m*p + b;
}
const matrix<double,2,2>& get_m(
) const { return m; }
const dlib::vector<double,2>& get_b(
) const { return b; }
inline friend void serialize (const point_transform_affine& item, std::ostream& out)
{
serialize(item.m, out);
serialize(item.b, out);
}
inline friend void deserialize (point_transform_affine& item, std::istream& in)
{
deserialize(item.m, in);
deserialize(item.b, in);
}
private:
matrix<double,2,2> m;
dlib::vector<double,2> b;
};
// ----------------------------------------------------------------------------------------
class rectangle_transform
{
public:
rectangle_transform (
)
{
}
rectangle_transform (
const point_transform_affine& tform_
) :tform(tform_)
{
}
drectangle operator() (
const drectangle& r
) const
{
dpoint tl = r.tl_corner();
dpoint tr = r.tr_corner();
dpoint bl = r.bl_corner();
dpoint br = r.br_corner();
// The new rectangle would ideally have this area if we could actually rotate
// the box. Note the 1+ is because that's how the rectangles calculate their
// width and height.
double new_area = (1+length(tform(tl)-tform(tr)))*(1+length(tform(tl)-tform(bl)));
// But if we rotate the corners of the rectangle and then find the rectangle
// that contains them we get this, which might have a much larger area than we
// want.
drectangle temp;
temp += tform(tl);
temp += tform(tr);
temp += tform(bl);
temp += tform(br);
// so we adjust the area to match the target area and have the same center as
// the above box.
double scale = std::sqrt(new_area/temp.area());
return centered_drect(dcenter(temp), temp.width()*scale, temp.height()*scale);
}
rectangle operator() (
const rectangle& r
) const
{
const auto temp = (*this)(drectangle(r));
return centered_rect(center(temp), std::round(temp.width()), std::round(temp.height()));
}
const point_transform_affine& get_tform(
) const { return tform; }
inline friend void serialize (const rectangle_transform& item, std::ostream& out)
{
serialize(item.tform, out);
}
inline friend void deserialize (rectangle_transform& item, std::istream& in)
{
deserialize(item.tform, in);
}
private:
point_transform_affine tform;
};
// ----------------------------------------------------------------------------------------
inline point_transform_affine operator* (
const point_transform_affine& lhs,
const point_transform_affine& rhs
)
{
return point_transform_affine(lhs.get_m()*rhs.get_m(), lhs.get_m()*rhs.get_b()+lhs.get_b());
}
// ----------------------------------------------------------------------------------------
inline point_transform_affine inv (
const point_transform_affine& trans
)
{
matrix<double,2,2> im = inv(trans.get_m());
return point_transform_affine(im, -im*trans.get_b());
}
// ----------------------------------------------------------------------------------------
template <typename T>
point_transform_affine find_affine_transform (
const std::vector<dlib::vector<T,2> >& from_points,
const std::vector<dlib::vector<T,2> >& to_points
)
{
// make sure requires clause is not broken
DLIB_ASSERT(from_points.size() == to_points.size() &&
from_points.size() >= 3,
"\t point_transform_affine find_affine_transform(from_points, to_points)"
<< "\n\t Invalid inputs were given to this function."
<< "\n\t from_points.size(): " << from_points.size()
<< "\n\t to_points.size(): " << to_points.size()
);
matrix<double,3,0> P(3, from_points.size());
matrix<double,2,0> Q(2, from_points.size());
for (unsigned long i = 0; i < from_points.size(); ++i)
{
P(0,i) = from_points[i].x();
P(1,i) = from_points[i].y();
P(2,i) = 1;
Q(0,i) = to_points[i].x();
Q(1,i) = to_points[i].y();
}
const matrix<double,2,3> m = Q*pinv(P);
return point_transform_affine(subm(m,0,0,2,2), colm(m,2));
}
// ----------------------------------------------------------------------------------------
template <typename T>
point_transform_affine find_similarity_transform (
const std::vector<dlib::vector<T,2> >& from_points,
const std::vector<dlib::vector<T,2> >& to_points
)
{
// make sure requires clause is not broken
DLIB_ASSERT(from_points.size() == to_points.size() &&
from_points.size() >= 2,
"\t point_transform_affine find_similarity_transform(from_points, to_points)"
<< "\n\t Invalid inputs were given to this function."
<< "\n\t from_points.size(): " << from_points.size()
<< "\n\t to_points.size(): " << to_points.size()
);
// We use the formulas from the paper: Least-squares estimation of transformation
// parameters between two point patterns by Umeyama. They are equations 34 through
// 43.
dlib::vector<double,2> mean_from, mean_to;
double sigma_from = 0, sigma_to = 0;
matrix<double,2,2> cov;
cov = 0;
for (unsigned long i = 0; i < from_points.size(); ++i)
{
mean_from += from_points[i];
mean_to += to_points[i];
}
mean_from /= from_points.size();
mean_to /= from_points.size();
for (unsigned long i = 0; i < from_points.size(); ++i)
{
sigma_from += length_squared(from_points[i] - mean_from);
sigma_to += length_squared(to_points[i] - mean_to);
cov += (to_points[i] - mean_to)*trans(from_points[i] - mean_from);
}
sigma_from /= from_points.size();
sigma_to /= from_points.size();
cov /= from_points.size();
matrix<double,2,2> u, v, s, d;
svd(cov, u,d,v);
s = identity_matrix(cov);
if (det(cov) < 0 || (det(cov) == 0 && det(u)*det(v)<0))
{
if (d(1,1) < d(0,0))
s(1,1) = -1;
else
s(0,0) = -1;
}
matrix<double,2,2> r = u*s*trans(v);
double c = 1;
if (sigma_from != 0)
c = 1.0/sigma_from * trace(d*s);
vector<double,2> t = mean_to - c*r*mean_from;
return point_transform_affine(c*r, t);
}
// ----------------------------------------------------------------------------------------
class point_transform_projective
{
public:
point_transform_projective (
)
{
m = identity_matrix<double>(3);
}
point_transform_projective (
const matrix<double,3,3>& m_
) :m(m_)
{
}
point_transform_projective (
const point_transform_affine& tran
)
{
set_subm(m, 0,0, 2,2) = tran.get_m();
set_subm(m, 0,2, 2,1) = tran.get_b();
m(2,0) = 0;
m(2,1) = 0;
m(2,2) = 1;
}
const dlib::vector<double,2> operator() (
const dlib::vector<double,2>& p
) const
{
dlib::vector<double,3> temp(p);
temp.z() = 1;
temp = m*temp;
if (temp.z() != 0)
temp = temp/temp.z();
return temp;
}
const matrix<double,3,3>& get_m(
) const { return m; }
inline friend void serialize (const point_transform_projective& item, std::ostream& out)
{
serialize(item.m, out);
}
inline friend void deserialize (point_transform_projective& item, std::istream& in)
{
deserialize(item.m, in);
}
private:
matrix<double,3,3> m;
};
// ----------------------------------------------------------------------------------------
inline point_transform_projective operator* (
const point_transform_projective& lhs,
const point_transform_projective& rhs
)
{
return point_transform_projective(lhs.get_m()*rhs.get_m());
}
// ----------------------------------------------------------------------------------------
inline point_transform_projective inv (
const point_transform_projective& trans
)
{
return point_transform_projective(inv(trans.get_m()));
}
// ----------------------------------------------------------------------------------------
namespace impl_proj
{
inline point_transform_projective find_projective_transform_basic (
const std::vector<dlib::vector<double,2> >& from_points,
const std::vector<dlib::vector<double,2> >& to_points
)
/*!
ensures
- Uses the system of equations approach to finding a projective transform.
This is "Method 3" from Estimating Projective Transformation Matrix by
Zhengyou Zhang.
- It should be emphasized that the find_projective_transform_basic()
routine, which uses the most popular method for finding projective
transformations, doesn't really work well when the minimum error solution
doesn't have zero error. In this case, it can deviate by a large amount
from the proper minimum mean squared error transformation. Therefore,
our overall strategy will be to use the solution from
find_projective_transform_basic() as a starting point for a BFGS based
non-linear optimizer which will optimize the correct mean squared error
criterion.
A great essay on this subject is Homography Estimation by Elan Dubrofsky.
!*/
{
// make sure requires clause is not broken
DLIB_ASSERT(from_points.size() == to_points.size() &&
from_points.size() >= 4,
"\t point_transform_projective find_projective_transform_basic(from_points, to_points)"
<< "\n\t Invalid inputs were given to this function."
<< "\n\t from_points.size(): " << from_points.size()
<< "\n\t to_points.size(): " << to_points.size()
);
matrix<double,9,9> accum, u, v;
matrix<double,9,1> w;
matrix<double,2,9> B;
accum = 0;
B = 0;
for (unsigned long i = 0; i < from_points.size(); ++i)
{
dlib::vector<double,3> f = from_points[i];
f.z() = 1;
dlib::vector<double,3> t = to_points[i];
t.z() = 1;
set_subm(B,0,0,1,3) = t.y()*trans(f);
set_subm(B,1,0,1,3) = trans(f);
set_subm(B,0,3,1,3) = -t.x()*trans(f);
set_subm(B,1,6,1,3) = -t.x()*trans(f);
accum += trans(B)*B;
}
svd2(true, false, accum, u, w, v);
long j = index_of_min(w);
return point_transform_projective(reshape(colm(u,j),3,3));
}
// ----------------------------------------------------------------------------------------
struct obj
{
/*!
WHAT THIS OBJECT REPRESENTS
This is the objective function we really want to minimize when looking
for a transformation matrix. That is, we would like the transformed
points to be as close as possible to their "to" points. Here,
closeness is measured using Euclidean distance.
!*/
obj(
const std::vector<dlib::vector<double,2> >& from_points_,
const std::vector<dlib::vector<double,2> >& to_points_
) :
from_points(from_points_) ,
to_points(to_points_)
{}
const std::vector<dlib::vector<double,2> >& from_points;
const std::vector<dlib::vector<double,2> >& to_points;
double operator() (
const matrix<double,9,1>& p
) const
{
point_transform_projective tran(reshape(p,3,3));
double sum = 0;
for (unsigned long i = 0; i < from_points.size(); ++i)
{
sum += length_squared(tran(from_points[i]) - to_points[i]);
}
return sum;
}
};
struct obj_der
{
/*!
WHAT THIS OBJECT REPRESENTS
This is the derivative of obj.
!*/
obj_der(
const std::vector<dlib::vector<double,2> >& from_points_,
const std::vector<dlib::vector<double,2> >& to_points_
) :
from_points(from_points_) ,
to_points(to_points_)
{}
const std::vector<dlib::vector<double,2> >& from_points;
const std::vector<dlib::vector<double,2> >& to_points;
matrix<double,9,1> operator() (
const matrix<double,9,1>& p
) const
{
const matrix<double,3,3> H = reshape(p,3,3);
matrix<double,3,3> grad;
grad = 0;
for (unsigned long i = 0; i < from_points.size(); ++i)
{
dlib::vector<double,3> from, to;
from = from_points[i];
from.z() = 1;
to = to_points[i];
to.z() = 1;
matrix<double,3,1> w = H*from;
const double scale = (w(2) != 0) ? (1.0/w(2)) : (1);
w *= scale;
matrix<double,3,1> residual = (w-to)*2*scale;
grad(0,0) += from.x()*residual(0);
grad(0,1) += from.y()*residual(0);
grad(0,2) += residual(0);
grad(1,0) += from.x()*residual(1);
grad(1,1) += from.y()*residual(1);
grad(1,2) += residual(1);
grad(2,0) += -(from.x()*w(0)*residual(0) + from.x()*w(1)*residual(1));
grad(2,1) += -(from.y()*w(0)*residual(0) + from.y()*w(1)*residual(1));
grad(2,2) += -( w(0)*residual(0) + w(1)*residual(1));
}
return reshape_to_column_vector(grad);
}
};
}
// ----------------------------------------------------------------------------------------
inline point_transform_projective find_projective_transform (
const std::vector<dlib::vector<double,2> >& from_points,
const std::vector<dlib::vector<double,2> >& to_points
)
{
using namespace impl_proj;
// make sure requires clause is not broken
DLIB_ASSERT(from_points.size() == to_points.size() &&
from_points.size() >= 4,
"\t point_transform_projective find_projective_transform(from_points, to_points)"
<< "\n\t Invalid inputs were given to this function."
<< "\n\t from_points.size(): " << from_points.size()
<< "\n\t to_points.size(): " << to_points.size()
);
// Find a candidate projective transformation. Also, find the best affine
// transform and then compare it with the projective transform estimated using the
// direct SVD method. Use whichever one works better as the starting point for a
// BFGS based optimizer. If the best solution has large mean squared error and is
// also close to affine then find_projective_transform_basic() might give a very
// bad initial guess. So also checking for a good affine transformation can
// produce a much better final result in many cases.
point_transform_projective tran1 = find_projective_transform_basic(from_points, to_points);
point_transform_affine tran2 = find_affine_transform(from_points, to_points);
// check which is best
double error1 = 0;
double error2 = 0;
for (unsigned long i = 0; i < from_points.size(); ++i)
{
error1 += length_squared(tran1(from_points[i])-to_points[i]);
error2 += length_squared(tran2(from_points[i])-to_points[i]);
}
matrix<double,9,1> params;
// Pick the minimum error solution among the two so far.
if (error1 < error2)
params = reshape_to_column_vector(tran1.get_m());
else
params = reshape_to_column_vector(point_transform_projective(tran2).get_m());
// Now refine the transformation matrix so that we can be sure we have
// at least a local minimizer.
obj o(from_points, to_points);
obj_der der(from_points, to_points);
find_min(bfgs_search_strategy(),
objective_delta_stop_strategy(1e-6,100),
o,
der,
params,
0);
return point_transform_projective(reshape(params,3,3));
}
// ----------------------------------------------------------------------------------------
template <typename T>
const dlib::vector<T,2> rotate_point (
const dlib::vector<T,2>& center,
const dlib::vector<T,2>& p,
double angle
)
{
point_rotator rot(angle);
return rot(p-center)+center;
}
// ----------------------------------------------------------------------------------------
inline matrix<double,2,2> rotation_matrix (
double angle
)
{
const double ca = std::cos(angle);
const double sa = std::sin(angle);
matrix<double,2,2> m;
m = ca, -sa,
sa, ca;
return m;
}
// ----------------------------------------------------------------------------------------
// ----------------------------------------------------------------------------------------
// ----------------------------------------------------------------------------------------
class point_transform_affine3d
{
public:
point_transform_affine3d (
)
{
m = identity_matrix<double>(3);
b.x() = 0;
b.y() = 0;
}
point_transform_affine3d (
const matrix<double,3,3>& m_,
const dlib::vector<double,3>& b_
) :m(m_), b(b_)
{
}
const dlib::vector<double,3> operator() (
const dlib::vector<double,3>& p
) const
{
return m*p + b;
}
const matrix<double,3,3>& get_m(
) const { return m; }
const dlib::vector<double,3>& get_b(
) const { return b; }
inline friend void serialize (const point_transform_affine3d& item, std::ostream& out)
{
serialize(item.m, out);
serialize(item.b, out);
}
inline friend void deserialize (point_transform_affine3d& item, std::istream& in)
{
deserialize(item.m, in);
deserialize(item.b, in);
}
private:
matrix<double,3,3> m;
dlib::vector<double,3> b;
};
// ----------------------------------------------------------------------------------------
inline point_transform_affine3d operator* (
const point_transform_affine3d& lhs,
const point_transform_affine& rhs
)
{
matrix<double,3,3> m;
m = 0;
set_subm(m, get_rect(rhs.get_m())) = rhs.get_m();
vector<double,3> b = rhs.get_b();
return point_transform_affine3d(lhs.get_m()*m, lhs.get_m()*b+lhs.get_b());
}
// ----------------------------------------------------------------------------------------
inline point_transform_affine3d operator* (
const point_transform_affine3d& lhs,
const point_transform_affine3d& rhs
)
{
return point_transform_affine3d(lhs.get_m()*rhs.get_m(), lhs.get_m()*rhs.get_b()+lhs.get_b());
}
// ----------------------------------------------------------------------------------------
inline point_transform_affine3d inv (
const point_transform_affine3d& trans
)
{
matrix<double,3,3> im = inv(trans.get_m());
return point_transform_affine3d(im, -im*trans.get_b());
}
// ----------------------------------------------------------------------------------------
inline point_transform_affine3d rotate_around_x (
double angle
)
{
const double ca = std::cos(angle);
const double sa = std::sin(angle);
matrix<double,3,3> m;
m = 1, 0, 0,
0, ca, -sa,
0, sa, ca;
vector<double,3> b;
return point_transform_affine3d(m,b);
}
// ----------------------------------------------------------------------------------------
inline point_transform_affine3d rotate_around_y (
double angle
)
{
const double ca = std::cos(angle);
const double sa = std::sin(angle);
matrix<double,3,3> m;
m = ca, 0, sa,
0, 1, 0,
-sa, 0, ca;
vector<double,3> b;
return point_transform_affine3d(m,b);
}
// ----------------------------------------------------------------------------------------
inline point_transform_affine3d rotate_around_z (
double angle
)
{
const double ca = std::cos(angle);
const double sa = std::sin(angle);
matrix<double,3,3> m;
m = ca, -sa, 0,
sa, ca, 0,
0, 0, 1;
vector<double,3> b;
return point_transform_affine3d(m,b);
}
// ----------------------------------------------------------------------------------------
inline point_transform_affine3d translate_point (
const vector<double,3>& delta
)
{
return point_transform_affine3d(identity_matrix<double>(3),delta);
}
inline point_transform_affine3d translate_point (
double x,
double y,
double z
)
{
return translate_point(vector<double>(x,y,z));
}
// ----------------------------------------------------------------------------------------
class camera_transform
{
public:
camera_transform (
)
{
*this = camera_transform(vector<double>(1,1,1),
vector<double>(0,0,0),
vector<double>(0,0,1),
90,
1);
}
camera_transform (
const vector<double>& camera_pos_,
const vector<double>& camera_looking_at_,
const vector<double>& camera_up_direction_,
const double camera_field_of_view_,
const unsigned long num_pixels_
)
{
// make sure requires clause is not broken
DLIB_CASSERT(0 < camera_field_of_view_ && camera_field_of_view_ < 180,
"\t camera_transform::camera_transform()"
<< "\n\t Invalid inputs were given to this function."
<< "\n\t camera_field_of_view_: " << camera_field_of_view_
);
camera_pos = camera_pos_;
camera_looking_at = camera_looking_at_;
camera_up_direction = camera_up_direction_;
camera_field_of_view = camera_field_of_view_;
num_pixels = num_pixels_;
dlib::vector<double> X,Y,Z;
Z = (camera_looking_at - camera_pos).normalize();
Y = camera_up_direction - dot(camera_up_direction,Z)*Z;
Y = Y.normalize();
X = Z.cross(Y);
set_rowm(proj,0) = trans(X);
// Minus because images have y axis going down but we want the 3d projection to appear using a normal coordinate system with y going up.
set_rowm(proj,1) = -trans(Y);
set_rowm(proj,2) = trans(Z);
width = num_pixels/2.0;
dist_scale = width/std::tan(pi/180*camera_field_of_view/2);
}
vector<double> get_camera_pos() const { return camera_pos; }
vector<double> get_camera_looking_at() const { return camera_looking_at; }
vector<double> get_camera_up_direction()const { return camera_up_direction; }
double get_camera_field_of_view() const { return camera_field_of_view; }
unsigned long get_num_pixels() const { return num_pixels; }
inline dpoint operator() (
const vector<double>& p,
double& scale,
double& distance
) const
{
vector<double> temp = p-camera_pos;
temp = proj*temp;
distance = temp.z();
scale = dist_scale/(temp.z()>0 ? temp.z() : 1e-9);
temp.x() = temp.x()*scale + width;
temp.y() = temp.y()*scale + width;
return temp;
}
dpoint operator() (
const vector<double>& p
) const
{
double scale, distance;
return (*this)(p,scale,distance);
}
inline friend void serialize (const camera_transform& item, std::ostream& out)
{
serialize(item.camera_pos, out);
serialize(item.camera_looking_at, out);
serialize(item.camera_up_direction, out);
serialize(item.camera_field_of_view, out);
serialize(item.num_pixels, out);
serialize(item.proj, out);
serialize(item.dist_scale, out);
serialize(item.width, out);
}
inline friend void deserialize (camera_transform& item, std::istream& in)
{
deserialize(item.camera_pos, in);
deserialize(item.camera_looking_at, in);
deserialize(item.camera_up_direction, in);
deserialize(item.camera_field_of_view, in);
deserialize(item.num_pixels, in);
deserialize(item.proj, in);
deserialize(item.dist_scale, in);
deserialize(item.width, in);
}
private:
vector<double> camera_pos;
vector<double> camera_looking_at;
vector<double> camera_up_direction;
double camera_field_of_view;
unsigned long num_pixels;
matrix<double,3,3> proj;
double dist_scale;
double width;
};
// ----------------------------------------------------------------------------------------
}
#endif // DLIB_POINT_TrANSFORMS_H_