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;
}
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 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);
}
}