Warp MT::Lucas_Kanade(Warp warp)
{
for (int iter = 0; iter < max_iteration; ++iter) {
Matx61f G;
Matx<float, 6, 6> H;
G = 0.0f;
H = 0.0f;
float E = 0.0f;
for (int i = 0; i < fine_samples.size(); ++i) {
Matx<float, L4, 1> T(fine_model.ptr<float>(i)), F;
Matx<float, 2, L4> dF;
Matx<float, 2, 6> dW = warp.gradient(fine_samples[i]);
Point2f p = warp.transform2(fine_samples[i]);
feature.gradient4(p.x, p.y, F.val, dF.val, dF.val + L4);
T -= F;
float e = sigmoid(T.dot(T));
E += e;
float w = sigmoid_factor * e * (1.0f - e);
G += w * (dW.t() * (dF * T));
H += w * (dW.t() * (dF * dF.t()) * dW);
}
E = E / fine_samples.size();
Matx61f D;
solve(H, G, D, DECOMP_SVD);
warp.steepest(D);
if (iter > 1 && D(3) * D(3) + D(4) * D(4) + D(5) * D(5) < translate_eps) {
if (log != NULL)
(*log) << "\terror in iteration " << iter << " = " << E << endl;
break;
}
}
return warp;
}
template<typename T> inline Matx<T, 3, 3> get_Aff_mat(const Matx<T, 3, 3>& invVR1_m,
const Matx<T, 3, 3>& invVR2_m)
{
//const Matx<double, 3, 3> V1_m = invVR1_m.inv();
//const Matx<double, 3, 3> Aff_mat = invVR2_m * V1_m;
const Matx<double, 3, 3> Aff_mat = invVR2_m * invVR1_m.inv();
return Aff_mat;
}
// Matx case
template<typename _Tp, int _rows> static inline
void cv2eigen( const Matx<_Tp, _rows, 1>& src,
Eigen::Matrix<_Tp, Eigen::Dynamic, 1>& dst )
{
dst.resize(_rows);
if( !(dst.Flags & Eigen::RowMajorBit) )
{
const Mat _dst(1, _rows, DataType<_Tp>::type,
dst.data(), (size_t)(dst.stride()*sizeof(_Tp)));
transpose(src, _dst);
}
else
{
const Mat _dst(_rows, 1, DataType<_Tp>::type,
dst.data(), (size_t)(dst.stride()*sizeof(_Tp)));
src.copyTo(_dst);
}
}
void MapperGradShift::calculate(
const cv::Mat& img1, const cv::Mat& image2, cv::Ptr<Map>& res) const
{
Mat gradx, grady, imgDiff;
Mat img2;
CV_DbgAssert(img1.size() == image2.size());
if(!res.empty()) {
// We have initial values for the registration: we move img2 to that initial reference
res->inverseWarp(image2, img2);
} else {
img2 = image2;
}
// Get gradient in all channels
gradient(img1, img2, gradx, grady, imgDiff);
// Calculate parameters using least squares
Matx<double, 2, 2> A;
Vec<double, 2> b;
// For each value in A, all the matrix elements are added and then the channels are also added,
// so we have two calls to "sum". The result can be found in the first element of the final
// Scalar object.
A(0, 0) = sum(sum(gradx.mul(gradx)))[0];
A(0, 1) = sum(sum(gradx.mul(grady)))[0];
A(1, 1) = sum(sum(grady.mul(grady)))[0];
A(1, 0) = A(0, 1);
b(0) = -sum(sum(imgDiff.mul(gradx)))[0];
b(1) = -sum(sum(imgDiff.mul(grady)))[0];
// Calculate shift. We use Cholesky decomposition, as A is symmetric.
Vec<double, 2> shift = A.inv(DECOMP_CHOLESKY)*b;
if(res.empty()) {
res = new MapShift(shift);
} else {
MapShift newTr(shift);
res->compose(newTr);
}
}
void cv2eigen( const Matx<_Tp, _rows, 1>& src,
Eigen::Matrix<_Tp, Eigen::Dynamic, 1>& dst )
{
dst.resize(_rows);
if( !(dst.Flags & Eigen::RowMajorBit) )
{
Mat _dst(1, _rows, DataType<_Tp>::type,
dst.data(), (size_t)(dst.stride()*sizeof(_Tp)));
transpose(src, _dst);
CV_DbgAssert(_dst.data == (uchar*)dst.data());
}
else
{
Mat _dst(_rows, 1, DataType<_Tp>::type,
dst.data(), (size_t)(dst.stride()*sizeof(_Tp)));
src.copyTo(_dst);
CV_DbgAssert(_dst.data == (uchar*)dst.data());
}
}
void MapperGradAffine::calculate(
const cv::Mat& img1, const cv::Mat& image2, cv::Ptr<Map>& res) const
{
Mat gradx, grady, imgDiff;
Mat img2;
CV_DbgAssert(img1.size() == image2.size());
CV_DbgAssert(img1.channels() == image2.channels());
CV_DbgAssert(img1.channels() == 1 || img1.channels() == 3);
if(!res.empty()) {
// We have initial values for the registration: we move img2 to that initial reference
res->inverseWarp(image2, img2);
} else {
img2 = image2;
}
// Get gradient in all channels
gradient(img1, img2, gradx, grady, imgDiff);
// Matrices with reference frame coordinates
Mat grid_r, grid_c;
grid(img1, grid_r, grid_c);
// Calculate parameters using least squares
Matx<double, 6, 6> A;
Vec<double, 6> b;
// For each value in A, all the matrix elements are added and then the channels are also added,
// so we have two calls to "sum". The result can be found in the first element of the final
// Scalar object.
Mat xIx = grid_c.mul(gradx);
Mat xIy = grid_c.mul(grady);
Mat yIx = grid_r.mul(gradx);
Mat yIy = grid_r.mul(grady);
Mat Ix2 = gradx.mul(gradx);
Mat Iy2 = grady.mul(grady);
Mat xy = grid_c.mul(grid_r);
Mat IxIy = gradx.mul(grady);
A(0, 0) = sum(sum(sqr(xIx)))[0];
A(0, 1) = sum(sum(xy.mul(Ix2)))[0];
A(0, 2) = sum(sum(grid_c.mul(Ix2)))[0];
A(0, 3) = sum(sum(sqr(grid_c).mul(IxIy)))[0];
A(0, 4) = sum(sum(xy.mul(IxIy)))[0];
A(0, 5) = sum(sum(grid_c.mul(IxIy)))[0];
A(1, 1) = sum(sum(sqr(yIx)))[0];
A(1, 2) = sum(sum(grid_r.mul(Ix2)))[0];
A(1, 3) = A(0, 4);
A(1, 4) = sum(sum(sqr(grid_r).mul(IxIy)))[0];
A(1, 5) = sum(sum(grid_r.mul(IxIy)))[0];
A(2, 2) = sum(sum(Ix2))[0];
A(2, 3) = A(0, 5);
A(2, 4) = A(1, 5);
A(2, 5) = sum(sum(IxIy))[0];
A(3, 3) = sum(sum(sqr(xIy)))[0];
A(3, 4) = sum(sum(xy.mul(Iy2)))[0];
A(3, 5) = sum(sum(grid_c.mul(Iy2)))[0];
A(4, 4) = sum(sum(sqr(yIy)))[0];
A(4, 5) = sum(sum(grid_r.mul(Iy2)))[0];
A(5, 5) = sum(sum(Iy2))[0];
// Lower half values (A is symmetric)
A(1, 0) = A(0, 1);
A(2, 0) = A(0, 2);
A(2, 1) = A(1, 2);
A(3, 0) = A(0, 3);
A(3, 1) = A(1, 3);
A(3, 2) = A(2, 3);
A(4, 0) = A(0, 4);
A(4, 1) = A(1, 4);
A(4, 2) = A(2, 4);
A(4, 3) = A(3, 4);
A(5, 0) = A(0, 5);
A(5, 1) = A(1, 5);
A(5, 2) = A(2, 5);
A(5, 3) = A(3, 5);
A(5, 4) = A(4, 5);
// Calculation of b
b(0) = -sum(sum(imgDiff.mul(xIx)))[0];
b(1) = -sum(sum(imgDiff.mul(yIx)))[0];
b(2) = -sum(sum(imgDiff.mul(gradx)))[0];
b(3) = -sum(sum(imgDiff.mul(xIy)))[0];
b(4) = -sum(sum(imgDiff.mul(yIy)))[0];
b(5) = -sum(sum(imgDiff.mul(grady)))[0];
// Calculate affine transformation. We use Cholesky decomposition, as A is symmetric.
Vec<double, 6> k = A.inv(DECOMP_CHOLESKY)*b;
Matx<double, 2, 2> linTr(k(0) + 1., k(1), k(3), k(4) + 1.);
Vec<double, 2> shift(k(2), k(5));
if(res.empty()) {
res = Ptr<Map>(new MapAffine(linTr, shift));
} else {
MapAffine newTr(linTr, shift);
res->compose(newTr);
}
}
void MapperGradEuclid::calculate(
const cv::Mat& img1, const cv::Mat& image2, cv::Ptr<Map>& res) const
{
Mat gradx, grady, imgDiff;
Mat img2;
CV_DbgAssert(img1.size() == image2.size());
CV_DbgAssert(img1.channels() == image2.channels());
CV_DbgAssert(img1.channels() == 1 || img1.channels() == 3);
if(!res.empty()) {
// We have initial values for the registration: we move img2 to that initial reference
res->inverseWarp(image2, img2);
} else {
img2 = image2;
}
// Matrices with reference frame coordinates
Mat grid_r, grid_c;
grid(img1, grid_r, grid_c);
// Get gradient in all channels
gradient(img1, img2, gradx, grady, imgDiff);
// Calculate parameters using least squares
Matx<double, 3, 3> A;
Vec<double, 3> b;
// For each value in A, all the matrix elements are added and then the channels are also added,
// so we have two calls to "sum". The result can be found in the first element of the final
// Scalar object.
Mat xIy_yIx = grid_c.mul(grady);
xIy_yIx -= grid_r.mul(gradx);
A(0, 0) = sum(sum(gradx.mul(gradx)))[0];
A(0, 1) = sum(sum(gradx.mul(grady)))[0];
A(0, 2) = sum(sum(gradx.mul(xIy_yIx)))[0];
A(1, 1) = sum(sum(grady.mul(grady)))[0];
A(1, 2) = sum(sum(grady.mul(xIy_yIx)))[0];
A(2, 2) = sum(sum(xIy_yIx.mul(xIy_yIx)))[0];
A(1, 0) = A(0, 1);
A(2, 0) = A(0, 2);
A(2, 1) = A(1, 2);
b(0) = -sum(sum(imgDiff.mul(gradx)))[0];
b(1) = -sum(sum(imgDiff.mul(grady)))[0];
b(2) = -sum(sum(imgDiff.mul(xIy_yIx)))[0];
// Calculate parameters. We use Cholesky decomposition, as A is symmetric.
Vec<double, 3> k = A.inv(DECOMP_CHOLESKY)*b;
double cosT = cos(k(2));
double sinT = sin(k(2));
Matx<double, 2, 2> linTr(cosT, -sinT, sinT, cosT);
Vec<double, 2> shift(k(0), k(1));
if(res.empty()) {
res = Ptr<Map>(new MapAffine(linTr, shift));
} else {
MapAffine newTr(linTr, shift);
res->compose(newTr);
}
}
