void serialize(Archive & ar, ::cv::Mat_<T>& m, const unsigned int version)
{
if(Archive::is_loading::value == true)
{
int cols, rows;
size_t elem_size, elem_type;
ar & cols;
ar & rows;
ar & elem_size;
ar & elem_type;
m.create(rows, cols);
size_t data_size = m.cols * m.rows * elem_size;
ar & boost::serialization::make_array(m.ptr(), data_size);
}
else
{
size_t elem_size = m.elemSize();
size_t elem_type = m.type();
ar & m.cols;
ar & m.rows;
ar & elem_size;
ar & elem_type;
const size_t data_size = m.cols * m.rows * elem_size;
ar & boost::serialization::make_array(m.ptr(), data_size);
}
}
void writeMat(cv::Mat_<T>& mat, char* file){
ofstream* ofile = new ofstream(file,ofstream::out|ofstream::binary);
(*ofile)<<mat.rows<<" ";
(*ofile)<<mat.cols<<" ";
(*ofile)<<mat.type()<<" ";
(*ofile)<<mat.total()*mat.elemSize();
ofile->write((char*) mat.data,mat.total()*mat.elemSize());
ofile->close();
}
void
extendMat(cv::Mat_<int> &m, unsigned int rows, unsigned int cols, int value = 0) {
cv::Size2i interSize;
interSize.height = std::min<int>(rows, m.rows);
interSize.width = std::min<int>(cols, m.cols);
cv::Mat tm(rows, cols, m.type());
tm.setTo(cv::Scalar(value));
if(interSize.width!=0 && interSize.height!=0)
m(cv::Rect(cv::Point(0,0), interSize)).copyTo(tm(cv::Rect(cv::Point(0,0), interSize)));
m = tm;
}
void ColorEdge::detectColorEdge(const cv::Mat_<cv::Vec3b> &image, cv::Mat_<uchar> &edge)
{
cv::Mat_<double> edge_map(image.size());
const int filter_half = static_cast<int>(filter_size_ / 2);
for(int y = filter_half; y < (edge_map.rows - filter_half); ++y)
{
for(int x = filter_half; x < (edge_map.cols - filter_half); ++x)
{
cv::Mat_<cv::Vec3b> roi(image, cv::Rect(x - filter_half, y - filter_half, filter_size_, filter_size_));
edge_map(y, x) = calculateMVD(roi);
}
}
edge_map.convertTo(edge, edge.type());
}
//===========================================================================
// Calculate the PDM's Jacobian over rigid parameters (rotation, translation and scaling), the additional input W represents trust for each of the landmarks and is part of Non-Uniform RLMS
void PDM::ComputeRigidJacobian(const cv::Mat_<float>& p_local, const cv::Vec6d& params_global, cv::Mat_<float> &Jacob, const cv::Mat_<float> W, cv::Mat_<float> &Jacob_t_w)
{
// number of verts
int n = this->NumberOfPoints();
Jacob.create(n * 2, 6);
float X,Y,Z;
float s = (float)params_global[0];
cv::Mat_<double> shape_3D_d;
cv::Mat_<double> p_local_d;
p_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);
// Get the rotation matrix
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 = Jacob.begin();
cv::MatIterator_<float> Jy = Jx + n * 6;
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;
}
cv::Mat Jacob_w = cv::Mat::zeros(Jacob.rows, Jacob.cols, Jacob.type());
Jx = Jacob.begin();
Jy = Jx + n*6;
cv::MatIterator_<float> Jx_w = Jacob_w.begin<float>();
cv::MatIterator_<float> Jy_w = Jx_w + n*6;
// 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 < Jacob.cols; ++j)
{
*Jx_w++ = *Jx++ * w_x;
*Jy_w++ = *Jy++ * w_y;
}
}
Jacob_t_w = Jacob_w.t();
}
