void GrayWorldEstimator::preprocessImage(const cv::Mat_<cv::Vec3d>& image, const cv::Mat_<unsigned char>& mask, cv::Mat_<cv::Vec3d> &inputImage, cv::Mat_<unsigned char> &inputMask) const
{
inputImage = image.clone();
inputMask = mask.clone();
if ((image.rows != mask.rows) || (image.cols != mask.cols)) {
inputMask = cv::Mat_<unsigned char>(inputImage.rows, inputImage.cols, (unsigned char)0);
}
Mask::maskSaturatedPixels(inputImage, inputMask, 1);
cv::Mat_<unsigned char> element = cv::Mat_<unsigned char>::ones(3, 3);
cv::dilate(inputMask, inputMask, element);
const double kernelsize = cvRound(m_sigma * 3 * 2 + 1) | 1;
Mask::maskBorderPixels(inputImage, inputMask, (kernelsize + 1) / 2);
if (m_sigma > 0) {
if (m_n == 0) {
const double kernelsize = cvRound(m_sigma * 3 * 2 + 1) | 1;
cv::GaussianBlur(inputImage, inputImage, cv::Size(kernelsize, kernelsize), m_sigma, m_sigma);
} else if (m_n > 0) {
inputImage = Derivative::normDerivativeFilter(inputImage, m_n, m_sigma);
}
}
}
void FaceAnalyser::UpdateRunningMedian(cv::Mat_<unsigned int>& histogram, int& hist_count, cv::Mat_<double>& median, const cv::Mat_<double>& descriptor, bool update, int num_bins, double min_val, double max_val)
{
double length = max_val - min_val;
if(length < 0)
length = -length;
// The median update
if(histogram.empty())
{
histogram = Mat_<unsigned int>(descriptor.cols, num_bins, (unsigned int)0);
median = descriptor.clone();
}
if(update)
{
// Find the bins corresponding to the current descriptor
Mat_<double> converted_descriptor = (descriptor - min_val)*((double)num_bins)/(length);
// Capping the top and bottom values
converted_descriptor.setTo(Scalar(num_bins-1), converted_descriptor > num_bins - 1);
converted_descriptor.setTo(Scalar(0), converted_descriptor < 0);
// Only count the median till a certain number of frame seen?
for(int i = 0; i < histogram.rows; ++i)
{
int index = (int)converted_descriptor.at<double>(i);
histogram.at<unsigned int>(i, index)++;
}
// Update the histogram count
hist_count++;
}
if(hist_count == 1)
{
median = descriptor.clone();
}
else
{
// Recompute the median
int cutoff_point = (hist_count + 1)/2;
// For each dimension
for(int i = 0; i < histogram.rows; ++i)
{
int cummulative_sum = 0;
for(int j = 0; j < histogram.cols; ++j)
{
cummulative_sum += histogram.at<unsigned int>(i, j);
if(cummulative_sum > cutoff_point)
{
median.at<double>(i) = min_val + j * (length/num_bins) + (0.5*(length)/num_bins);
break;
}
}
}
}
}
void Tan::preprocessImage(const cv::Mat_<cv::Vec3d>& image, const cv::Mat_<unsigned char>& mask, cv::Mat_<cv::Vec3d> &outputImage, cv::Mat_<unsigned char> &outputMask) const
{
outputImage = image.clone();
if ((image.rows != mask.rows) || (image.cols != mask.cols)) {
std::cerr << "No mask!" << std::endl;
outputMask = cv::Mat_<unsigned char>::zeros(outputImage.rows, outputImage.cols);
} else {
outputMask = mask.clone();
}
illumestimators::Mask::maskSaturatedPixels(outputImage, outputMask, config.max_intensity);
illumestimators::Mask::maskDarkPixels(outputImage, outputMask, config.min_intensity);
}
//=============================================================================
// Basically Kabsch's algorithm but also allows the collection of points to be different in scale from each other
cv::Matx22f AlignShapesWithScale(cv::Mat_<float>& src, cv::Mat_<float> dst)
{
int n = src.rows;
// First we mean normalise both src and dst
float mean_src_x = cv::mean(src.col(0))[0];
float mean_src_y = cv::mean(src.col(1))[0];
float mean_dst_x = cv::mean(dst.col(0))[0];
float mean_dst_y = cv::mean(dst.col(1))[0];
cv::Mat_<float> src_mean_normed = src.clone();
src_mean_normed.col(0) = src_mean_normed.col(0) - mean_src_x;
src_mean_normed.col(1) = src_mean_normed.col(1) - mean_src_y;
cv::Mat_<float> dst_mean_normed = dst.clone();
dst_mean_normed.col(0) = dst_mean_normed.col(0) - mean_dst_x;
dst_mean_normed.col(1) = dst_mean_normed.col(1) - mean_dst_y;
// Find the scaling factor of each
cv::Mat src_sq;
cv::pow(src_mean_normed, 2, src_sq);
cv::Mat dst_sq;
cv::pow(dst_mean_normed, 2, dst_sq);
float s_src = sqrt(cv::sum(src_sq)[0] / n);
float s_dst = sqrt(cv::sum(dst_sq)[0] / n);
src_mean_normed = src_mean_normed / s_src;
dst_mean_normed = dst_mean_normed / s_dst;
float s = s_dst / s_src;
// Get the rotation
cv::Matx22f R = AlignShapesKabsch2D(src_mean_normed, dst_mean_normed);
cv::Matx22f A;
cv::Mat(s * R).copyTo(A);
cv::Mat_<float> aligned = (cv::Mat(cv::Mat(A) * src.t())).t();
cv::Mat_<float> offset = dst - aligned;
float t_x = cv::mean(offset.col(0))[0];
float t_y = cv::mean(offset.col(1))[0];
return A;
}
cv::Mat_<float> midpoint(cv::Mat_<float> &img, unsigned int filter_size)
{
cv::Mat_<float> img_filter = img.clone();
std::vector<float> filter_list(filter_size*filter_size, 0.0);
unsigned int temp_x, temp_y, i = 0, percentile_i;
for(size_t y = 0; y < img.cols; y++)
{
for(size_t x = 0; x < img.rows; x++)
{
for(size_t filter_y = 0; filter_y < filter_size; ++filter_y)
{
for(size_t filter_x = 0; filter_x < filter_size; ++filter_x)
{
temp_x = (x - filter_size / 2 + filter_x + img.rows) % img.rows;
temp_y = (y - filter_size / 2 + filter_y + img.cols) % img.cols;
filter_list[i] = img(temp_x, temp_y);
i++;
}
}
i = 0;
std::sort(filter_list.begin(), filter_list.end());
img_filter(x, y) = (filter_list.front() + filter_list.back())/2;
}
}
return img_filter;
}
/** Orthogonal matching pursuit
* x: input signal, N * 1
* D: dictionary, N * M
* L: number of non_zero elements in output
* coeff: coefficent of each atoms in dictionary, M * 1
*/
void OMP(const cv::Mat_<double>& x, const cv::Mat_<double>& D, int L, cv::Mat_<double>& coeff){
int dim = x.rows;
int atom_num = D.cols;
coeff = Mat::zeros(atom_num, 1, CV_64FC1);
Mat_<double> residual = x.clone();
Mat_<double> selected_index(L, 1);
Mat_<double> a;
for (int i = 0; i < L; i++){
cout << "here ok 1" << endl;
Mat_<double> dot_p = D.t() * residual;
Point max_index;
minMaxLoc(abs(dot_p), NULL, NULL, NULL, &max_index);
int max_row = max_index.y;
selected_index(i) = max_row;
Mat_<double> temp(dim, i + 1);
for (int j = 0; j < i + 1; j++){
D.col(selected_index(j)).copyTo(temp.col(j));
}
Mat_<double> invert_temp;
invert(temp, invert_temp, CV_SVD);
a = invert_temp * x;
residual = x - temp * a;
}
for (int i = 0; i < L; i++){
coeff(selected_index(i)) = a(i);
}
}
cv::Mat_<float> get_DFT_image(cv::Mat_<float> &img, bool log)
{
int rows = cv::getOptimalDFTSize(2 * img.rows);
int cols = cv::getOptimalDFTSize(2 * img.cols);
int imgRows = img.rows;
int imgCols = img.cols;
cv::copyMakeBorder(img, img, 0, rows - img.rows, 0, cols - img.cols, cv::BORDER_CONSTANT, cv::Scalar(0));
cv::Mat_<float> imgs[] = {img.clone(), cv::Mat_<float>(img.rows, img.cols, 0.0f)};
cv::Mat_<cv::Vec2f> img_dft;
cv::merge(imgs, 2, img_dft);
cv::dft(img_dft, img_dft);
cv::split(img_dft, imgs);
cv::Mat_<float> magnitude, phase;
cv::cartToPolar(imgs[0], imgs[1], magnitude, phase);
dftshift(magnitude);
if(log)
{
magnitude += 1.0f;
cv::log(magnitude, magnitude);
}
img = img(cv::Rect(0,0,imgCols,imgRows));
return magnitude;
}
cv::Mat_<float> cModel::Reshape_alt(cv::Mat_<float>& mean, cv::Rect& faceBox)
{
cv::Mat_<double> modelShape = mean.clone();
cv::Mat_<double> xCoords = modelShape.colRange(0, modelShape.cols / 2);
cv::Mat_<double> yCoords = modelShape.colRange(modelShape.cols / 2, modelShape.cols);
double minX, maxX, minY, maxY;
cv::minMaxLoc(xCoords, &minX, &maxX);//得到x的最大/最小值
cv::minMaxLoc(yCoords, &minY, &maxY);//得到y的最大/最小值
double faceboxScaleFactor = m_Params->__facebox_scale_factor;
double modelWidth = maxX - minX;
double modelHeight = maxY - minY;
xCoords -= minX;
yCoords -= minY;
// scale it:
xCoords *= faceBox.width / modelWidth;
yCoords *= faceBox.height / modelHeight;
// modelShape = modelShape * (faceBox.width / modelWidth + faceBox.height / modelHeight) / (params->__facebox_scale_const * faceboxScaleFactor);
// translate the model:
// cv::Scalar meanX = cv::mean(xCoords);
// double meanXd = meanX[0];
// cv::Scalar meanY = cv::mean(yCoords);
// double meanYd = meanY[0];
// move it:
xCoords += faceBox.x; // +faceBox.width / params->__facebox_width_div - meanXd;
yCoords += faceBox.y; // +faceBox.height / params->__facebox_height_div - meanYd;
return modelShape;
}
/**
* Find simplfied homography representation from rotation and translation
* @param h**o: [Output] Estimated Homography
* @param rot: [Input] Rotation result
* @param trans: [Input] Translation result
*/
void AugmentationEnvironment::Rt2H(const cv::Mat_<double> intrisinc, const cv::Mat_<double>& rot, const cv::Mat_<double> &trans, cv::Mat_<double>& h**o)
{
cv::Mat_<double> R = rot.clone();
R.at<double>(0,2) = trans.at<double>(0,0);
R.at<double>(1,2) = trans.at<double>(1,0);
R.at<double>(2,2) = trans.at<double>(2,0);
h**o = (intrisinc*R)/trans.at<double>(2,0);
}
void SegmenterHumanSimple::_prob2energy(const cv::Mat_<float>& prob, cv::Mat_<float>& fgdEnergy, cv::Mat_<float>& bgdEnergy)
{
fgdEnergy = prob.clone();
bgdEnergy = prob.clone();
cv::log(prob,fgdEnergy);
cv::log(1-prob,bgdEnergy);
fgdEnergy = -fgdEnergy;
bgdEnergy = -bgdEnergy;
}
double LinearRegression::train(const cv::Mat_<double>& inputs, const cv::Mat_<double>& Y) {
cv::Mat_<double> X = inputs.clone();
ml::addBias(X);
W = X.inv(cv::DECOMP_SVD) * Y;
// residueの計算
cv::Mat_<double> avg_mat;
cv::reduce((X * W - Y).mul(X * W - Y), avg_mat, 1, CV_REDUCE_SUM);
cv::reduce(avg_mat, avg_mat, 0, CV_REDUCE_AVG);
return sqrt(avg_mat(0, 0));
}
cv::Mat_<float> DescriptorJoiner::loadDescriptors( const string& dfile, int* label) throw (DescriptorLengthException)
{
const cv::Mat_<float> vecs = RFeatures::readDescriptors( dfile, false);
const int numVecs = vecs.rows;
const int lab = (int)_labCounts.size(); // Label for these desciptors
_labCounts.push_back(numVecs); // Store the number of descriptors for this class label (vector index)
// Add vecs to _xs
for ( int i = 0; i < numVecs; ++i)
{
_xs.push_back( vecs.row(i));
_labs.push_back(lab);
} // end for
return vecs.clone();
} // end loadDescriptors
cv::Mat_<double> ClusteredLinearRegression::predict(const cv::Mat_<double>& x) {
// 直近のクラスタを探す
float min_dist = std::numeric_limits<float>::max();
int min_id = -1;
for (int i = 0; i < clusterCentroids.size(); ++i) {
float dist = cv::norm((clusterCentroids[i] - x));
if (dist < min_dist) {
min_dist = dist;
min_id = i;
}
}
cv::Mat_<double> x2 = x.clone();
ml::addBias(x2);
return x2 * W[min_id];
}
void spm_bp::ModifyCrossMapArmlengthToFitSubImage(const cv::Mat_<cv::Vec4b>& crMapIn, int maxArmLength, cv::Mat_<cv::Vec4b>& crMapOut)
{
int iy, ix, height, width;
height = crMapIn.rows;
width = crMapIn.cols;
crMapOut = crMapIn.clone();
// up
for (iy = 0; iy < min<int>(maxArmLength, height); ++iy) {
for (ix = 0; ix < width; ++ix) {
crMapOut[iy][ix][1] = min<int>(iy, crMapOut[iy][ix][1]);
}
}
// down
int ky = maxArmLength - 1;
for (iy = height - maxArmLength; iy < height; ++iy) {
if (iy < 0) {
--ky;
continue;
}
for (ix = 0; ix < width; ++ix) {
crMapOut[iy][ix][3] = min<int>(ky, crMapOut[iy][ix][3]);
}
--ky;
}
// left
for (iy = 0; iy < height; ++iy) {
for (ix = 0; ix < min<int>(width, maxArmLength); ++ix) {
crMapOut[iy][ix][0] = min<int>(ix, crMapOut[iy][ix][0]);
}
}
// right
int kx;
for (iy = 0; iy < height; ++iy) {
kx = maxArmLength - 1;
for (ix = width - maxArmLength; ix < width; ++ix) {
if (ix < 0) {
--kx;
continue;
}
crMapOut[iy][ix][2] = min<int>(kx, crMapOut[iy][ix][2]);
--kx;
}
}
}
cv::Mat_<cv::Vec3f> Utility::getChromacityImage(cv::Mat_<cv::Vec3f>& rgbImage) {
///normalize r,g,b channels
cv::Size imgSize=rgbImage.size();
cv::Mat_<float> normImage(imgSize);
normImage.setTo(cv::Scalar(0, 0, 0));
cv::Mat_<cv::Vec3f> chromacityImage=rgbImage.clone();
std::vector<cv::Mat_<float> > rgbPlanes;
cv::split(rgbImage, rgbPlanes);
cv::Mat_<float> singlePlane=normImage.clone();
for (int i=0; i<3; i++) {
cv::pow(rgbPlanes[i], 2.0, singlePlane);
cv::add(normImage, singlePlane, normImage);
}
cv::sqrt(normImage, normImage);
for (int i=0; i<3; i++) {
cv::divide(rgbPlanes[i], normImage, rgbPlanes[i]);
}
cv::merge(&rgbPlanes[0], 3, chromacityImage);
return chromacityImage;
}
/**
* @brief Util::clearFragment
* @brief 消除图像的小块碎片，先DFS遍历，若小于阈值，则DFS替换
* @param image 输入输出图像
* @return 没有返回值
*/
void Util::clearFragment(cv::Mat_<cv::Vec3b>& image){
std::cout << "start clearFragment" << std::endl;
cv::Vec3b white = cv::Vec3b(255,255,255);
Util::imageCopy = image.clone();
for(int y_offset = 0; y_offset < Util::imageCopy.rows; y_offset++){
for(int x_offset = 0; x_offset < Util::imageCopy.cols; x_offset++){
cv::Vec3b currentColor = Util::imageCopy.at<cv::Vec3b>(y_offset,x_offset);
if(currentColor == white){
continue;
}
Util::framentSizeCount = 0;
Util::calcFramentSize(image,cv::Point(x_offset,y_offset));
std::cout << "calcFramentSize result:" << Util::framentSizeCount << " x,y=" << x_offset <<"," <<y_offset << std::endl;
if(Util::framentSizeCount < 64){
std::cout << "Util::framentSizeCount < 64" << std::endl;
Util::replaceColorBlockDFS(image,cv::Point(x_offset,y_offset),white);
}
}
}
}
// Pick only the more stable/rigid points under changes of expression
void extract_rigid_points(cv::Mat_<double>& source_points, cv::Mat_<double>& destination_points)
{
if(source_points.rows == 68)
{
cv::Mat_<double> tmp_source = source_points.clone();
source_points = cv::Mat_<double>();
// Push back the rigid points (some face outline, eyes, and nose)
source_points.push_back(tmp_source.row(1));
source_points.push_back(tmp_source.row(2));
source_points.push_back(tmp_source.row(3));
source_points.push_back(tmp_source.row(4));
source_points.push_back(tmp_source.row(12));
source_points.push_back(tmp_source.row(13));
source_points.push_back(tmp_source.row(14));
source_points.push_back(tmp_source.row(15));
source_points.push_back(tmp_source.row(27));
source_points.push_back(tmp_source.row(28));
source_points.push_back(tmp_source.row(29));
source_points.push_back(tmp_source.row(31));
source_points.push_back(tmp_source.row(32));
source_points.push_back(tmp_source.row(33));
source_points.push_back(tmp_source.row(34));
source_points.push_back(tmp_source.row(35));
source_points.push_back(tmp_source.row(36));
source_points.push_back(tmp_source.row(39));
source_points.push_back(tmp_source.row(40));
source_points.push_back(tmp_source.row(41));
source_points.push_back(tmp_source.row(42));
source_points.push_back(tmp_source.row(45));
source_points.push_back(tmp_source.row(46));
source_points.push_back(tmp_source.row(47));
cv::Mat_<double> tmp_dest = destination_points.clone();
destination_points = cv::Mat_<double>();
// Push back the rigid points
destination_points.push_back(tmp_dest.row(1));
destination_points.push_back(tmp_dest.row(2));
destination_points.push_back(tmp_dest.row(3));
destination_points.push_back(tmp_dest.row(4));
destination_points.push_back(tmp_dest.row(12));
destination_points.push_back(tmp_dest.row(13));
destination_points.push_back(tmp_dest.row(14));
destination_points.push_back(tmp_dest.row(15));
destination_points.push_back(tmp_dest.row(27));
destination_points.push_back(tmp_dest.row(28));
destination_points.push_back(tmp_dest.row(29));
destination_points.push_back(tmp_dest.row(31));
destination_points.push_back(tmp_dest.row(32));
destination_points.push_back(tmp_dest.row(33));
destination_points.push_back(tmp_dest.row(34));
destination_points.push_back(tmp_dest.row(35));
destination_points.push_back(tmp_dest.row(36));
destination_points.push_back(tmp_dest.row(39));
destination_points.push_back(tmp_dest.row(40));
destination_points.push_back(tmp_dest.row(41));
destination_points.push_back(tmp_dest.row(42));
destination_points.push_back(tmp_dest.row(45));
destination_points.push_back(tmp_dest.row(46));
destination_points.push_back(tmp_dest.row(47));
}
}
void FaceAnalyser::PredictAUs(const cv::Mat_<double>& hog_features, const cv::Mat_<double>& geom_features, const CLMTracker::CLM& clm_model, bool online)
{
// Store the descriptor
hog_desc_frame = hog_features.clone();
this->geom_descriptor_frame = geom_features.clone();
Vec3d curr_orient(clm_model.params_global[1], clm_model.params_global[2], clm_model.params_global[3]);
int orientation_to_use = GetViewId(this->head_orientations, curr_orient);
// Perform AU prediction
AU_predictions_reg = PredictCurrentAUs(orientation_to_use);
std::vector<std::pair<std::string, double>> AU_predictions_reg_corrected;
if(online)
{
AU_predictions_reg_corrected = CorrectOnlineAUs(AU_predictions_reg, orientation_to_use, true, false, clm_model.detection_success);
}
// Keep only closer to in-plane faces
double angle_norm = cv::sqrt(clm_model.params_global[2] * clm_model.params_global[2] + clm_model.params_global[3] * clm_model.params_global[3]);
// Add the reg predictions to the historic data
for (size_t au = 0; au < AU_predictions_reg.size(); ++au)
{
// Find the appropriate AU (if not found add it)
// Only add if the detection was successful and not too out of plane
if(clm_model.detection_success && angle_norm < 0.4)
{
AU_predictions_reg_all_hist[AU_predictions_reg[au].first].push_back(AU_predictions_reg[au].second);
}
else
{
AU_predictions_reg_all_hist[AU_predictions_reg[au].first].push_back(-100.0);
}
}
AU_predictions_class = PredictCurrentAUsClass(orientation_to_use);
for (size_t au = 0; au < AU_predictions_class.size(); ++au)
{
// Find the appropriate AU (if not found add it)
// Only add if the detection was successful and not too out of plane
if(clm_model.detection_success && angle_norm < 0.4)
{
AU_predictions_class_all_hist[AU_predictions_class[au].first].push_back(AU_predictions_class[au].second);
}
else
{
AU_predictions_class_all_hist[AU_predictions_class[au].first].push_back(-100.0);
}
}
if(online)
{
AU_predictions_reg = AU_predictions_reg_corrected;
}
for(size_t i = 0; i < AU_predictions_reg.size(); ++i)
{
AU_predictions_combined.push_back(AU_predictions_reg[i]);
}
for(size_t i = 0; i < AU_predictions_class.size(); ++i)
{
AU_predictions_combined.push_back(AU_predictions_class[i]);
}
view_used = orientation_to_use;
bool success = clm_model.detection_success && angle_norm < 0.4;
confidences.push_back(clm_model.detection_certainty);
valid_preds.push_back(success);
}
cv::Mat LinearRegression::predict(const cv::Mat_<double>& inputs) {
cv::Mat_<double> X = inputs.clone();
ml::addBias(X);
return X * W;
}
//===========================================================================
// Calculate the PDM's Jacobian over all parameters (rigid and non-rigid), the additional input W represents trust for each of the landmarks and is part of Non-Uniform RLMS
void PDM::ComputeJacobian(const cv::Mat_<float>& params_local, const cv::Vec6d& params_global, cv::Mat_<float> &Jacobian, const cv::Mat_<float> W, cv::Mat_<float> &Jacob_t_w)
{
// number of vertices
int n = this->NumberOfPoints();
// number of non-rigid parameters
int m = this->NumberOfModes();
Jacobian.create(n * 2, 6 + m);
float X,Y,Z;
float s = (float) params_global[0];
cv::Mat_<double> shape_3D_d;
cv::Mat_<double> p_local_d;
params_local.convertTo(p_local_d, CV_64F);
this->CalcShape3D(shape_3D_d, p_local_d);
cv::Mat_<float> shape_3D;
shape_3D_d.convertTo(shape_3D, CV_32F);
cv::Vec3d euler(params_global[1], params_global[2], params_global[3]);
cv::Matx33d currRot = Euler2RotationMatrix(euler);
float r11 = (float) currRot(0,0);
float r12 = (float) currRot(0,1);
float r13 = (float) currRot(0,2);
float r21 = (float) currRot(1,0);
float r22 = (float) currRot(1,1);
float r23 = (float) currRot(1,2);
float r31 = (float) currRot(2,0);
float r32 = (float) currRot(2,1);
float r33 = (float) currRot(2,2);
cv::MatIterator_<float> Jx = Jacobian.begin();
cv::MatIterator_<float> Jy = Jx + n * (6 + m);
cv::MatConstIterator_<double> Vx = this->princ_comp.begin();
cv::MatConstIterator_<double> Vy = Vx + n*m;
cv::MatConstIterator_<double> Vz = Vy + n*m;
for(int i = 0; i < n; i++)
{
X = shape_3D.at<float>(i,0);
Y = shape_3D.at<float>(i+n,0);
Z = shape_3D.at<float>(i+n*2,0);
// The rigid jacobian from the axis angle rotation matrix approximation using small angle assumption (R * R')
// where R' = [1, -wz, wy
// wz, 1, -wx
// -wy, wx, 1]
// And this is derived using the small angle assumption on the axis angle rotation matrix parametrisation
// scaling term
*Jx++ = (X * r11 + Y * r12 + Z * r13);
*Jy++ = (X * r21 + Y * r22 + Z * r23);
// rotation terms
*Jx++ = (s * (Y * r13 - Z * r12) );
*Jy++ = (s * (Y * r23 - Z * r22) );
*Jx++ = (-s * (X * r13 - Z * r11));
*Jy++ = (-s * (X * r23 - Z * r21));
*Jx++ = (s * (X * r12 - Y * r11) );
*Jy++ = (s * (X * r22 - Y * r21) );
// translation terms
*Jx++ = 1.0f;
*Jy++ = 0.0f;
*Jx++ = 0.0f;
*Jy++ = 1.0f;
for(int j = 0; j < m; j++,++Vx,++Vy,++Vz)
{
// How much the change of the non-rigid parameters (when object is rotated) affect 2D motion
*Jx++ = (float) ( s*(r11*(*Vx) + r12*(*Vy) + r13*(*Vz)) );
*Jy++ = (float) ( s*(r21*(*Vx) + r22*(*Vy) + r23*(*Vz)) );
}
}
// Adding the weights here
cv::Mat Jacob_w = Jacobian.clone();
if(cv::trace(W)[0] != W.rows)
{
Jx = Jacobian.begin();
Jy = Jx + n*(6+m);
cv::MatIterator_<float> Jx_w = Jacob_w.begin<float>();
cv::MatIterator_<float> Jy_w = Jx_w + n*(6+m);
// Iterate over all Jacobian values and multiply them by the weight in diagonal of W
for(int i = 0; i < n; i++)
{
float w_x = W.at<float>(i, i);
float w_y = W.at<float>(i+n, i+n);
for(int j = 0; j < Jacobian.cols; ++j)
{
*Jx_w++ = *Jx++ * w_x;
*Jy_w++ = *Jy++ * w_y;
}
}
}
Jacob_t_w = Jacob_w.t();
}
void MeanEstimator::preprocessImage(const cv::Mat_<cv::Vec3d>& image, const cv::Mat_<unsigned char>& mask, cv::Mat_<cv::Vec3d> &outputImage, cv::Mat_<unsigned char> &outputMask) const
{
outputImage = image.clone();
outputMask = mask.clone();
}
cv::Mat_<float> adaptive_median_filter(cv::Mat_<float> &img, unsigned int max_filter_size)
{
cv::Mat_<float> img_filter = img.clone();
std::vector<float> filter_list;
unsigned int temp_x,
temp_y,
i = 0,
filter_size,
z_min,
z_max,
z_med;
for(size_t y = 0; y < img.cols; y++)
{
for(size_t x = 0; x < img.rows; x++)
{
filter_size = 3;
while(true)
{
filter_list.resize(filter_size*filter_size);
for(size_t filter_y = 0; filter_y < filter_size; ++filter_y)
{
for(size_t filter_x = 0; filter_x < filter_size; ++filter_x)
{
temp_x = (x - filter_size / 2 + filter_x + img.rows) % img.rows;
temp_y = (y - filter_size / 2 + filter_y + img.cols) % img.cols;
filter_list[i] = img(temp_x, temp_y);
i++;
}
}
i = 0;
std::sort(filter_list.begin(), filter_list.end());
z_min = filter_list[0];
z_max = filter_list[filter_size*filter_size - 1];
z_med = filter_list[filter_size*filter_size/2];
if(z_min == z_med || z_med == z_max)
{
filter_size += 2;
}
else
{
if(img(x, y) == z_min || img(x, y) == z_max)
{
img_filter(x, y) = z_med;
}
else
{
img_filter(x, y) = img(x, y);
}
break;
}
if(max_filter_size == filter_size)
{
img_filter(x, y) = img(x, y);
break;
}
}
}
}
return img_filter;
}