void findEssentialMatrix(MFramePair& pair, Mat_<double> K) {
vector<Point2f> k1, k2,n1,n2;
vector<int> usedIndex1,usedIndex2;
vector<uchar> status(pair.imgpts1.size());
Mat F = findFundamentalMat(pair.matchPts1, pair.matchPts2, CV_FM_RANSAC, 0.2, 0.9,
status);
Mat_<double> E;
E = K.t() * F * K; //according to HZ (9.12)
for (unsigned int i = 0; i < status.size(); i++) { // queryIdx is the "left" image
if (status[i]) {
usedIndex1.push_back(pair.matchedIndex1[i]);
k1.push_back(pair.matchPts1[i]);
usedIndex2.push_back(pair.matchedIndex2[i]);
k2.push_back(pair.matchPts2[i]);
}
}
correctMatches(F,k1,k2,n1,n2);
pair.matchPts1 = n1;
pair.matchPts2 = n2;
pair.matchedIndex1=usedIndex1;
pair.matchedIndex2=usedIndex2;
pair.F = F;
pair.E = E;
}
float TrainableStatModel::leaveOneOutCrossValidation(const Mat_<float> &samples, const Mat_<int> &classes) {
int correctResults = 0;
Mat_<int> sampleIdx(samples.rows - 1, 1);
for (int i = 1; i < samples.rows; i++) {
sampleIdx(i - 1, 0) = i;
}
for (int i = 0; i < samples.rows; i++) {
this->clear();
this->train(samples, classes, sampleIdx);
int actual = (float)this->predict(samples.row(i));
if (actual == classes(i,0)) {
correctResults++;
}
cout<<"actual = "<<actual<<", expected = "<<classes(i,0)<<endl;
sampleIdx(i, 0) = i;
}
return (float)correctResults/(float)samples.rows;
}
static void drawOpticalFlow(const Mat_<Point2f>& flow, Mat& dst, float maxmotion = -1)
{
dst.create(flow.size(), CV_8UC3);
dst.setTo(Scalar::all(0));
// determine motion range:
float maxrad = maxmotion;
if (maxmotion <= 0)
{
maxrad = 1;
for (int y = 0; y < flow.rows; ++y)
{
for (int x = 0; x < flow.cols; ++x)
{
Point2f u = flow(y, x);
if (!isFlowCorrect(u))
continue;
maxrad = max(maxrad, sqrt(u.x * u.x + u.y * u.y));
}
}
}
for (int y = 0; y < flow.rows; ++y)
{
for (int x = 0; x < flow.cols; ++x)
{
Point2f u = flow(y, x);
if (isFlowCorrect(u))
dst.at<Vec3b>(y, x) = computeColor(u.x / maxrad, u.y / maxrad);
}
}
}
bool SimpleNN::predict(const Mat_<double> &test_X, Mat_<double> &result, string &err_msg){
Mat_<double> input_data = test_X.reshape(0, test_X.rows*test_X.cols); // make it column vector
if (input_data.rows != this->structure[0]){
err_msg = "wrong input size";
return false;
}
for (int row_index = 1; row_index < this->layers[0].rows; ++row_index){
this->layers[0](row_index, 0) = input_data(row_index-1, 0);
}
int num_layers = (int) this->layers.size();
for (int layer_id = 0; layer_id < num_layers - 2; ++layer_id){
Mat_<double> product = tanh(this->weights[layer_id]*this->layers[layer_id]);
for (int row_index = 1; row_index < this->layers[layer_id+1].rows; ++row_index){
this->layers[layer_id+1](row_index, 0) = product(row_index-1, 0);
}
}
// compute the output layer
{
int layer_id = num_layers - 2;
this->layers[layer_id + 1] = tanh(this->weights[layer_id] * this->layers[layer_id]);
}
result = this->layers[num_layers - 1]; // return last layers (output layer).
cout << "result:\n" << result << endl;
err_msg = "";
return true;
}
Mat optical_flow(const Mat_<float>& ImgA, const Mat_<float>& ImgB,
int num_it, float threshold) {
// Compute Gaussin Pyramid
int nl = 5;
float ds = 0.5;
stack<pair<Mat, Mat> > gp = compute_gaussian_pyramids(ImgA, ImgB, nl, ds);
Mat_<float> u = Mat::zeros(ImgA.size(), CV_32F);
Mat_<float> v = Mat::zeros(ImgA.size(), CV_32F);
while (!gp.empty()) {
Mat imgA = (gp.top()).first;
Mat imgB = (gp.top()).second;
// Warp the first image.
Mat_<float> imgBw = imgB.clone();
/* Compute warping here from u and v. */
imgBw = compute_warp(imgB, u, v, ds);
/* Compute the derivatives. */
Mat_<float> Ix, Iy, It;
compute_derivatives(imgBw, imgA, Ix, Iy, It); // papers
Mat_<float> du = Mat::zeros(imgA.size(), CV_32F);
Mat_<float> dv = Mat::zeros(imgA.size(), CV_32F);
for (int i = 0; i < 500; i ++)
iterative_computation(du, dv, Ix, Iy, It);
u = u - du;
v = v - dv;
gp.pop();
}
Mat Mflow = color_map(u, v);
return Mflow;
}
void NNLSOptimizer::scannls(const Mat& A, const Mat& b,Mat &x)
{
int iter = 0;
int m = A.size().height;
int n = A.size().width;
Mat_<double> AT = A.t();
double error = 1e-8;
Mat_<double> H = AT*A;
Mat_<double> f = -AT*b;
Mat_<double> x_old = Mat_<double>::zeros(n,1);
Mat_<double> x_new = Mat_<double>::zeros(n,1);
Mat_<double> mu_old = Mat_<double>::zeros(n,1);
Mat_<double> mu_new = Mat_<double>::zeros(n,1);
Mat_<double> r = Mat_<double>::zeros(n,1);
f.copyTo(mu_old);
while(iter < NNLS_MAX_ITER)
{
iter++;
for(int k=0;k<n;k++)
{
x_old.copyTo(x_new);
x_new(k,0) = std::max(0.0, x_old(k,0) - (mu_old(k,0)/H(k,k)) );
if(x_new(k,0) != x_old(k,0))
{
r = mu_old + (x_new(k,0) - x_old(k,0))*H.col(k);
r.copyTo(mu_new);
}
x_new.copyTo(x_old);
mu_new.copyTo(mu_old);
}
if(eKKT(H,f,x_new,error) == true)
{
break;
}
}
x_new.copyTo(x);
}
void SegmenterHumanSimple::segment(const cv::Mat& img, Mat_<uchar>& mask)
{
Mat imgBGR;
Mat imgLAB;
Mat imgBGRo;
float rate = 500.0f/img.cols;
GaussianBlur(img,imgBGRo,Size(),0.8,0.8);
vector<Rect> faces;
resize(imgBGRo,imgBGRo,Size(),rate,rate);
cv::CascadeClassifier faceModel(this->_m_filenameFaceModel);
faceModel.detectMultiScale(imgBGRo,faces);
imgBGRo.convertTo( imgBGR, CV_32F, 1.0/255. );
cvtColor( imgBGR, imgLAB, CV_BGR2Lab );
Superpixel sp(1000,1,5);
Mat_<int> segmentation = sp.segment(imgLAB);
vector<SuperpixelStatistic> stat = sp.stat(imgLAB,imgBGR,segmentation);
Mat_<float> prob;
this->getPixelProbability(imgBGRo,prob,faces);
Mat_<float> sprob;
UtilsSuperpixel::Stat(segmentation,prob,stat,sprob);
Mat_<int> initial(int(stat.size()),1);
initial.setTo(1,sprob>0.5);
initial.setTo(0,sprob<=0.5);
Mat_<float> probaColor;
int myx = cv::countNonZero(initial);
this->_getColorProba(stat,initial,probaColor);
Mat_<float> fgdInit,bgdInit,fgdColor,bgdColor;
this->_prob2energy(sprob,fgdInit,bgdInit);
this->_prob2energy(probaColor,fgdColor,bgdColor);
Mat_<float> fgdEnergy, bgdEnergy;
fgdEnergy = fgdInit + fgdColor;
bgdEnergy = bgdInit + bgdColor;
Mat_<int> label;
mask.create(imgBGRo.rows,imgBGRo.cols);
UtilsSegmentation::MaxFlowSuperpixel(stat,fgdEnergy,bgdEnergy,50.0,label);
for( int i=0;i<mask.rows;i++)
{
for(int j=0;j<mask.cols;j++)
{
if ( label(segmentation(i,j)) > 0.5)
{
mask(i,j) = 255;
}
else
{
mask(i,j) = 0;
}
}
}
cv::resize(mask,mask,Size(img.cols,img.rows));
mask.setTo(255,mask>128);
mask.setTo(0,mask<=128);
}
void run(int /* start_from */ )
{
CvMat zeros;
memset(&zeros, 0, sizeof(zeros));
C_Caller caller, bad_caller;
CvMat objectPoints_c, r_vec_c, t_vec_c, A_c, distCoeffs_c, imagePoints_c,
dpdr_c, dpdt_c, dpdf_c, dpdc_c, dpdk_c;
const int n = 10;
Mat imagePoints_cpp(1, n, CV_32FC2); imagePoints_c = imagePoints_cpp;
Mat objectPoints_cpp(1, n, CV_32FC3);
randu(objectPoints_cpp, Scalar::all(1), Scalar::all(10));
objectPoints_c = objectPoints_cpp;
Mat t_vec_cpp(Mat::zeros(1, 3, CV_32F)); t_vec_c = t_vec_cpp;
Mat r_vec_cpp;
Rodrigues(Mat::eye(3, 3, CV_32F), r_vec_cpp); r_vec_c = r_vec_cpp;
Mat A_cpp = camMat.clone(); A_c = A_cpp;
Mat distCoeffs_cpp = distCoeffs.clone(); distCoeffs_c = distCoeffs_cpp;
Mat dpdr_cpp(2*n, 3, CV_32F); dpdr_c = dpdr_cpp;
Mat dpdt_cpp(2*n, 3, CV_32F); dpdt_c = dpdt_cpp;
Mat dpdf_cpp(2*n, 2, CV_32F); dpdf_c = dpdf_cpp;
Mat dpdc_cpp(2*n, 2, CV_32F); dpdc_c = dpdc_cpp;
Mat dpdk_cpp(2*n, 4, CV_32F); dpdk_c = dpdk_cpp;
caller.aspectRatio = 1.0;
caller.objectPoints = &objectPoints_c;
caller.r_vec = &r_vec_c;
caller.t_vec = &t_vec_c;
caller.A = &A_c;
caller.distCoeffs = &distCoeffs_c;
caller.imagePoints = &imagePoints_c;
caller.dpdr = &dpdr_c;
caller.dpdt = &dpdt_c;
caller.dpdf = &dpdf_c;
caller.dpdc = &dpdc_c;
caller.dpdk = &dpdk_c;
/********************/
int errors = 0;
bad_caller = caller;
bad_caller.objectPoints = 0;
errors += run_test_case( CV_StsBadArg, "Zero objectPoints", bad_caller );
bad_caller = caller;
bad_caller.r_vec = 0;
errors += run_test_case( CV_StsBadArg, "Zero r_vec", bad_caller );
bad_caller = caller;
bad_caller.t_vec = 0;
errors += run_test_case( CV_StsBadArg, "Zero t_vec", bad_caller );
bad_caller = caller;
bad_caller.A = 0;
errors += run_test_case( CV_StsBadArg, "Zero camMat", bad_caller );
bad_caller = caller;
bad_caller.imagePoints = 0;
errors += run_test_case( CV_StsBadArg, "Zero imagePoints", bad_caller );
/****************************/
Mat bad_r_vec_cpp1(r_vec_cpp.size(), CV_32S); CvMat bad_r_vec_c1 = bad_r_vec_cpp1;
Mat bad_r_vec_cpp2(2, 2, CV_32F); CvMat bad_r_vec_c2 = bad_r_vec_cpp2;
Mat bad_r_vec_cpp3(r_vec_cpp.size(), CV_32FC2); CvMat bad_r_vec_c3 = bad_r_vec_cpp3;
bad_caller = caller;
bad_caller.r_vec = &bad_r_vec_c1;
errors += run_test_case( CV_StsBadArg, "Bad rvec format", bad_caller );
bad_caller = caller;
bad_caller.r_vec = &bad_r_vec_c2;
errors += run_test_case( CV_StsBadArg, "Bad rvec format", bad_caller );
bad_caller = caller;
bad_caller.r_vec = &bad_r_vec_c3;
errors += run_test_case( CV_StsBadArg, "Bad rvec format", bad_caller );
/****************************/
Mat bad_t_vec_cpp1(t_vec_cpp.size(), CV_32S); CvMat bad_t_vec_c1 = bad_t_vec_cpp1;
Mat bad_t_vec_cpp2(2, 2, CV_32F); CvMat bad_t_vec_c2 = bad_t_vec_cpp2;
Mat bad_t_vec_cpp3(1, 1, CV_32FC2); CvMat bad_t_vec_c3 = bad_t_vec_cpp3;
bad_caller = caller;
bad_caller.t_vec = &bad_t_vec_c1;
errors += run_test_case( CV_StsBadArg, "Bad tvec format", bad_caller );
bad_caller = caller;
bad_caller.t_vec = &bad_t_vec_c2;
errors += run_test_case( CV_StsBadArg, "Bad tvec format", bad_caller );
bad_caller = caller;
bad_caller.t_vec = &bad_t_vec_c3;
errors += run_test_case( CV_StsBadArg, "Bad tvec format", bad_caller );
/****************************/
Mat bad_A_cpp1(A_cpp.size(), CV_32S); CvMat bad_A_c1 = bad_A_cpp1;
Mat bad_A_cpp2(2, 2, CV_32F); CvMat bad_A_c2 = bad_A_cpp2;
bad_caller = caller;
bad_caller.A = &bad_A_c1;
errors += run_test_case( CV_StsBadArg, "Bad A format", bad_caller );
bad_caller = caller;
bad_caller.A = &bad_A_c2;
errors += run_test_case( CV_StsBadArg, "Bad A format", bad_caller );
/****************************/
Mat bad_distCoeffs_cpp1(distCoeffs_cpp.size(), CV_32S); CvMat bad_distCoeffs_c1 = bad_distCoeffs_cpp1;
Mat bad_distCoeffs_cpp2(2, 2, CV_32F); CvMat bad_distCoeffs_c2 = bad_distCoeffs_cpp2;
Mat bad_distCoeffs_cpp3(1, 7, CV_32F); CvMat bad_distCoeffs_c3 = bad_distCoeffs_cpp3;
bad_caller = caller;
bad_caller.distCoeffs = &zeros;
errors += run_test_case( CV_StsBadArg, "Bad distCoeffs format", bad_caller );
bad_caller = caller;
bad_caller.distCoeffs = &bad_distCoeffs_c1;
errors += run_test_case( CV_StsBadArg, "Bad distCoeffs format", bad_caller );
bad_caller = caller;
bad_caller.distCoeffs = &bad_distCoeffs_c2;
errors += run_test_case( CV_StsBadArg, "Bad distCoeffs format", bad_caller );
bad_caller = caller;
bad_caller.distCoeffs = &bad_distCoeffs_c3;
errors += run_test_case( CV_StsBadArg, "Bad distCoeffs format", bad_caller );
/****************************/
Mat bad_dpdr_cpp1(dpdr_cpp.size(), CV_32S); CvMat bad_dpdr_c1 = bad_dpdr_cpp1;
Mat bad_dpdr_cpp2(dpdr_cpp.cols+1, 3, CV_32F); CvMat bad_dpdr_c2 = bad_dpdr_cpp2;
Mat bad_dpdr_cpp3(dpdr_cpp.cols, 7, CV_32F); CvMat bad_dpdr_c3 = bad_dpdr_cpp3;
bad_caller = caller;
bad_caller.dpdr = &zeros;
errors += run_test_case( CV_StsBadArg, "Bad dpdr format", bad_caller );
bad_caller = caller;
bad_caller.dpdr = &bad_dpdr_c1;
errors += run_test_case( CV_StsBadArg, "Bad dpdr format", bad_caller );
bad_caller = caller;
bad_caller.dpdr = &bad_dpdr_c2;
errors += run_test_case( CV_StsBadArg, "Bad dpdr format", bad_caller );
bad_caller = caller;
bad_caller.dpdr = &bad_dpdr_c3;
errors += run_test_case( CV_StsBadArg, "Bad dpdr format", bad_caller );
/****************************/
bad_caller = caller;
bad_caller.dpdt = &zeros;
errors += run_test_case( CV_StsBadArg, "Bad dpdt format", bad_caller );
bad_caller = caller;
bad_caller.dpdt = &bad_dpdr_c1;
errors += run_test_case( CV_StsBadArg, "Bad dpdt format", bad_caller );
bad_caller = caller;
bad_caller.dpdt = &bad_dpdr_c2;
errors += run_test_case( CV_StsBadArg, "Bad dpdt format", bad_caller );
bad_caller = caller;
bad_caller.dpdt = &bad_dpdr_c3;
errors += run_test_case( CV_StsBadArg, "Bad dpdt format", bad_caller );
/****************************/
Mat bad_dpdf_cpp2(dpdr_cpp.cols+1, 2, CV_32F); CvMat bad_dpdf_c2 = bad_dpdf_cpp2;
bad_caller = caller;
bad_caller.dpdf = &zeros;
errors += run_test_case( CV_StsBadArg, "Bad dpdf format", bad_caller );
bad_caller = caller;
bad_caller.dpdf = &bad_dpdr_c1;
errors += run_test_case( CV_StsBadArg, "Bad dpdf format", bad_caller );
bad_caller = caller;
bad_caller.dpdf = &bad_dpdf_c2;
errors += run_test_case( CV_StsBadArg, "Bad dpdf format", bad_caller );
bad_caller = caller;
bad_caller.dpdf = &bad_dpdr_c3;
errors += run_test_case( CV_StsBadArg, "Bad dpdf format", bad_caller );
/****************************/
bad_caller = caller;
bad_caller.dpdc = &zeros;
errors += run_test_case( CV_StsBadArg, "Bad dpdc format", bad_caller );
bad_caller = caller;
bad_caller.dpdc = &bad_dpdr_c1;
errors += run_test_case( CV_StsBadArg, "Bad dpdc format", bad_caller );
bad_caller = caller;
bad_caller.dpdc = &bad_dpdf_c2;
errors += run_test_case( CV_StsBadArg, "Bad dpdc format", bad_caller );
bad_caller = caller;
bad_caller.dpdc = &bad_dpdr_c3;
errors += run_test_case( CV_StsBadArg, "Bad dpdc format", bad_caller );
/****************************/
bad_caller = caller;
bad_caller.dpdk = &zeros;
errors += run_test_case( CV_StsBadArg, "Bad dpdk format", bad_caller );
bad_caller = caller;
bad_caller.dpdk = &bad_dpdr_c1;
errors += run_test_case( CV_StsBadArg, "Bad dpdk format", bad_caller );
bad_caller = caller;
bad_caller.dpdk = &bad_dpdf_c2;
errors += run_test_case( CV_StsBadArg, "Bad dpdk format", bad_caller );
bad_caller = caller;
bad_caller.dpdk = &bad_dpdr_c3;
errors += run_test_case( CV_StsBadArg, "Bad dpdk format", bad_caller );
bad_caller = caller;
bad_caller.distCoeffs = 0;
errors += run_test_case( CV_StsNullPtr, "distCoeffs is NULL while dpdk is not", bad_caller );
if (errors)
ts->set_failed_test_info(cvtest::TS::FAIL_MISMATCH);
else
ts->set_failed_test_info(cvtest::TS::OK);
}
//===========================================================================
void SVR_patch_expert::Response(const Mat_<float>& area_of_interest, Mat_<double>& response)
{
int response_height = area_of_interest.rows - weights.rows + 1;
int response_width = area_of_interest.cols - weights.cols + 1;
// the patch area on which we will calculate reponses
cv::Mat_<float> normalised_area_of_interest;
if(response.rows != response_height || response.cols != response_width)
{
response.create(response_height, response_width);
}
// If type is raw just normalise mean and standard deviation
if(type == 0)
{
// Perform normalisation across whole patch
cv::Scalar mean;
cv::Scalar std;
cv::meanStdDev(area_of_interest, mean, std);
// Avoid division by zero
if(std[0] == 0)
{
std[0] = 1;
}
normalised_area_of_interest = (area_of_interest - mean[0]) / std[0];
}
// If type is gradient, perform the image gradient computation
else if(type == 1)
{
Grad(area_of_interest, normalised_area_of_interest);
}
else
{
printf("ERROR(%s,%d): Unsupported patch type %d!\n", __FILE__,__LINE__, type);
abort();
}
Mat_<float> svr_response;
// The empty matrix as we don't pass precomputed dft's of image
Mat_<double> empty_matrix_0(0,0,0.0);
Mat_<float> empty_matrix_1(0,0,0.0);
Mat_<float> empty_matrix_2(0,0,0.0);
// Efficient calc of patch expert SVR response across the area of interest
matchTemplate_m(normalised_area_of_interest, empty_matrix_0, empty_matrix_1, empty_matrix_2, weights, weights_dfts, svr_response, CV_TM_CCOEFF_NORMED);
response.create(svr_response.size());
MatIterator_<double> p = response.begin();
cv::MatIterator_<float> q1 = svr_response.begin(); // respone for each pixel
cv::MatIterator_<float> q2 = svr_response.end();
while(q1 != q2)
{
// the SVR response passed into logistic regressor
*p++ = 1.0/(1.0 + exp( -(*q1++ * scaling + bias )));
}
}
/**
* @author JIA Pei
* @version 2010-05-20
* @brief Basic AAM Fitting, for dynamic image sequence
* @param iImg Input - image to be fitted
* @param ioShape Input and Output - the fitted shape
* @param oImg Output - the fitted image
* @param epoch Input - the iteration epoch
*/
float VO_FittingAAMBasic::VO_BasicAAMFitting(const Mat& iImg,
VO_Shape& ioShape,
Mat& oImg,
unsigned int epoch)
{
this->m_VOFittingShape.clone(ioShape);
double t = (double)cvGetTickCount();
this->SetProcessingImage(iImg, this->m_VOAAMBasic);
this->m_iIteration = 0;
// Get m_MatModelAlignedShapeParam and m_fScale, m_vRotateAngles, m_MatCenterOfGravity
this->m_VOAAMBasic->VO_CalcAllParams4AnyShapeWithConstrain( this->m_VOFittingShape,
this->m_MatModelAlignedShapeParam,
this->m_fScale,
this->m_vRotateAngles,
this->m_MatCenterOfGravity);
this->m_VOFittingShape.ConstrainShapeInImage(this->m_ImageProcessing);
// Get m_MatModelNormalizedTextureParam
VO_TextureModel::VO_LoadOneTextureFromShape(this->m_VOFittingShape,
this->m_ImageProcessing,
this->m_vTriangle2D,
this->m_vPointWarpInfo,
this->m_VOFittingTexture );
// estimate the texture model parameters
this->m_VOAAMBasic->VO_CalcAllParams4AnyTexture(this->m_VOFittingTexture, this->m_MatModelNormalizedTextureParam);
// Calculate m_MatCurrentC
this->m_VOAAMBasic->VO_SParamTParamProjectToCParam( this->m_MatModelAlignedShapeParam,
this->m_MatModelNormalizedTextureParam,
this->m_MatCurrentC );
// Set m_MatCurrentT, m_MatDeltaT, m_MatEstimatedT, m_MatDeltaC, m_MatEstimatedC, etc.
this->m_MatCurrentT = Mat_<float>::zeros(this->m_MatCurrentT.size());
this->m_MatDeltaT = Mat_<float>::zeros(this->m_MatDeltaT.size());
this->m_MatEstimatedT = Mat_<float>::zeros(this->m_MatEstimatedT.size());
this->m_MatDeltaC = Mat_<float>::zeros(this->m_MatDeltaC.size());
this->m_MatEstimatedC = Mat_<float>::zeros(this->m_MatEstimatedC.size());
//////////////////////////////////////////////////////////////////////////////////////////////////////
// explained by JIA Pei. 2010-05-20
// For the first round, this->m_VOFittingShape should not change after calling "VO_CParamTParam2FittingShape"
// But this is not the case. why?
// Before calling VO_CParamTParam2FittingShape, this->m_VOFittingShape is calculated by
// a) assigning m_VOTemplateAlignedShape
// b) align to the real-size face using detected eyes and mouth
// c) constrain the shape within the image
// d) constrain the shape parameters and calculate those rigid transform parameters
// cout << this->m_VOFittingShape << endl;
//////////////////////////////////////////////////////////////////////////////////////////////////////
// Estimate m_VOFittingShape and m_VOFittingTexture
this->VO_CParamTParam2FittingShape( this->m_MatCurrentC,
this->m_MatCurrentT,
this->m_VOModelNormalizedTexture,
this->m_VOFittingShape,
this->m_fScale,
this->m_vRotateAngles,
this->m_MatCenterOfGravity );
this->m_VOFittingShape.ConstrainShapeInImage(this->m_ImageProcessing); // Remember to call ConstrainShapeInImage() whenever you update m_VOFittingShape
//////////////////////////////////////////////////////////////////////////////////////////////////////
// When calling VO_CParamTParam2FittingShape, this->m_VOFittingShape is calculated by
// a) c parameters to reconstruct shape parameters
// b) shape parameters to reconstruct shape
// c) align to the real-size face by global shape normalization
// cout << this->m_VOFittingShape << endl;
//////////////////////////////////////////////////////////////////////////////////////////////////////
this->m_E_previous = this->m_E = this->VO_CalcErrorImage(this->m_ImageProcessing,
this->m_VOFittingShape,
this->m_VOModelNormalizedTexture,
this->m_VOTextureError);
do
{
float estScale = this->m_fScale;
vector<float> estRotateAngles = this->m_vRotateAngles;
Mat_<float> estCOG = this->m_MatCenterOfGravity.clone();
bool cBetter = false;
bool poseBetter = false;
/**First shape parameters, c parameters. refer to equation (9.3)
* Cootes "Statistical Model of Appearance for Computer Vision" */
cv::gemm(this->m_VOTextureError.GetTheTextureInARow(), this->m_VOAAMBasic->m_MatRc, -1, Mat(), 0.0, this->m_MatDeltaC, GEMM_2_T);
// damp -- C
for(unsigned int i = 0; i < k_values.size(); i++)
{
// make damped c prediction
cv::scaleAdd(this->m_MatDeltaC, k_values[i], this->m_MatCurrentC, this->m_MatEstimatedC);
// make sure m_MatEstimatedC are constrained
this->m_VOAAMBasic->VO_AppearanceParameterConstraint(this->m_MatEstimatedC);
this->VO_CParamTParam2FittingShape( this->m_MatEstimatedC,
this->m_MatCurrentT,
this->m_VOModelNormalizedTexture,
this->m_VOEstimatedShape,
estScale,
estRotateAngles,
estCOG);
if ( !VO_ShapeModel::VO_IsShapeInsideImage(this->m_VOEstimatedShape, this->m_ImageProcessing) )
continue;
else
this->m_E = this->VO_CalcErrorImage(this->m_ImageProcessing,
this->m_VOEstimatedShape,
this->m_VOModelNormalizedTexture,
this->m_VOEstimatedTextureError);
if (this->m_E < this->m_E_previous)
{
this->m_MatEstimatedC.copyTo(this->m_MatCurrentC);
this->m_VOFittingShape.clone(this->m_VOEstimatedShape);
this->m_VOTextureError.clone(this->m_VOEstimatedTextureError);
this->m_E_previous = this->m_E;
cBetter = true;
this->m_fScale = estScale;
this->m_vRotateAngles = estRotateAngles;
this->m_MatCenterOfGravity = estCOG.clone();
break;
}
}
/** Second pose, t parameters. refer to equation (9.3)
* Cootes "Statistical Model of Appearance for Computer Vision" */
cv::gemm(this->m_VOTextureError.GetTheTextureInARow(), this->m_VOAAMBasic->m_MatRt, -1, Mat(), 0, this->m_MatDeltaT, GEMM_2_T);
// damp -- T
for(unsigned int i = 0; i < k_values.size(); i++)
{
// make damped c/pose prediction
cv::scaleAdd(this->m_MatDeltaT, k_values[i], this->m_MatCurrentT, this->m_MatEstimatedT);
this->VO_CParamTParam2FittingShape( this->m_MatCurrentC,
this->m_MatEstimatedT,
this->m_VOModelNormalizedTexture,
this->m_VOEstimatedShape,
estScale,
estRotateAngles,
estCOG);
if ( !VO_ShapeModel::VO_IsShapeInsideImage(this->m_VOEstimatedShape, this->m_ImageProcessing) )
continue;
else
this->m_E = this->VO_CalcErrorImage(this->m_ImageProcessing,
this->m_VOEstimatedShape,
this->m_VOModelNormalizedTexture,
this->m_VOEstimatedTextureError);
if (this->m_E < this->m_E_previous)
{
// Since m_fScale, m_vRotateAngles and m_MatCenterOfGravity have been updated,
// m_MatCurrentT should be assigned to 0 now!
this->m_MatCurrentT = Mat_<float>::zeros(this->m_MatCurrentT.size());
// this->m_MatEstimatedT.copyTo(this->m_MatCurrentT);
this->m_VOFittingShape.clone(this->m_VOEstimatedShape);
this->m_VOTextureError.clone(this->m_VOEstimatedTextureError);
this->m_E_previous = this->m_E;
poseBetter = true;
this->m_fScale = estScale;
this->m_vRotateAngles = estRotateAngles;
this->m_MatCenterOfGravity = estCOG.clone();
break;
}
}
if( cBetter || poseBetter)
{
ioShape.clone(this->m_VOFittingShape);
}
else
break;
++this->m_iIteration;
}while( ( fabs(this->m_E) > FLT_EPSILON ) && (this->m_iIteration < epoch)/* && (cv::norm(this->m_MatDeltaC) > FLT_EPSILON) */ );
t = ((double)cvGetTickCount() - t )/ (cvGetTickFrequency()*1000.);
cout << "Basic fitting time cost: " << t << " millisec" << endl;
this->m_fFittingTime = t;
VO_Fitting2DSM::VO_DrawMesh(ioShape, this->m_VOAAMBasic, oImg);
return t;
}
int BaseDecisionTree::fit(Mat_<double> _X,
Mat_<double> _y,
Mat_<double> sample_weight)
{
// Validation
if (_X.rows == 0 || _X.cols == 0)
return 1;
// Determine output setting
_n_samples = _X.rows;
_n_features = _X.cols;
// Reshape y to shape[n_samples, 1]
_y = _y.reshape(1, _y.total());
// Validation
if (_y.rows != _n_samples)
return 2;
// Calculate class_weight
Mat expended_class_weight(0, 0, CV_32F);
// Get class_weight
if (_class_weight.total() != 0)
expended_class_weight = compute_sample_weight(_class_weight, _y);
// Validation
if (_max_depth <= 0)
_max_depth = static_cast<int>(pow(2, 31) - 1);
if (_max_leaf_nodes <= 0)
_max_leaf_nodes = -1;
if (_max_features <= 0)
_max_features = _n_features;
if (_max_leaf_nodes > -1 && _max_leaf_nodes < 2)
return 3;
if (_min_samples_split <= 0)
return 4;
if (_min_samples_leaf <= 0)
return 5;
if (_min_weight_fraction_leaf >= 0 && _min_weight_fraction_leaf <= 0.5)
return 6;
// Set samples' weight
if (expended_class_weight.total())
{
for (int i = 0; i < sample_weight.total(); i++)
{
sample_weight.at<double>(i, 0) = sample_weight.at<double>(i, 0) * \
expended_class_weight.at<double>(i, 0);
}
}
else
{
sample_weight = expended_class_weight;
}
// Set min_weight_fraction_leaf
if (_min_weight_fraction_leaf != 0.)
_min_weight_fraction_leaf = _min_weight_fraction_leaf * cv::sum(sample_weight);
else
_min_weight_fraction_leaf = 0.;
// Set min_samples_split
_min_samples_split = max(_min_samples_split, 2 * _min_samples_leaf);
}
/**
* @param avgSParam - input mean shape parameters
* @param icovSParam - input covariance matrix of shape parameters
* @param avgTParam - input mean texture parameters
* @param icovTParam - input covariance matrix of texture parameters
* @param iSParams - input the vector of multiple input shape parameters
* @param iTParams - input the vector of multiple input texture parameter
* @param ShapeDistMean - input mean texture parameters
* @param ShapeDistStddev - input covariance matrix of texture parameters
* @param TextureDistMean - input the input shape parameter
* @param TextureDistStddev - input the input texture parameter
* @param WeakFitting - input only shape parameter is used?
* @return whether the fitting is acceptable
*/
bool CRecognitionAlgs::CalcFittingEffect4ImageSequence(
const Mat_<float>& avgSParam,
const Mat_<float>& icovSParam,
const Mat_<float>& avgTParam,
const Mat_<float>& icovTParam,
const Mat_<float>& iSParams,
const Mat_<float>& iTParams,
const Scalar& ShapeDistMean,
const Scalar& ShapeDistStddev,
const Scalar& TextureDistMean,
const Scalar& TextureDistStddev,
bool WeakFitting )
{
assert(iSParams.rows == iTParams.rows);
unsigned int NbOfSamples = iSParams.rows;
vector<float> sDists, tDists;
sDists.resize(NbOfSamples);
tDists.resize(NbOfSamples);
for(unsigned int i = 0; i < NbOfSamples; ++i)
{
CRecognitionAlgs::CalcFittingEffect4StaticImage(
avgSParam,
icovSParam,
avgTParam,
icovTParam,
iSParams.row(i),
iTParams.row(i),
ShapeDistMean,
ShapeDistStddev,
TextureDistMean,
TextureDistStddev,
sDists[i],
tDists[i],
WeakFitting );
}
unsigned int NbOfGood1 = 0;
unsigned int NbOfGood2 = 0;
for(unsigned int i = 0; i < NbOfSamples; ++i)
{
if( ( fabs( sDists[i] - ShapeDistMean.val[0] )
< 1.5f * ShapeDistStddev.val[0] ) )
{
NbOfGood1++;
if( ( fabs( tDists[i] - TextureDistMean.val[0] )
< 3.0f*TextureDistStddev.val[0] ) )
{
NbOfGood2++;
}
}
}
if(WeakFitting)
{
if(NbOfGood1 >= (unsigned int )(0.75*NbOfSamples) )
return true;
else
return false;
}
else
{
if(NbOfGood2 >= (unsigned int )(0.75*NbOfGood1) )
return true;
else
return false;
}
}
void UtilsSegmentation::MaxFlowSuperpixel(std::vector<SuperpixelStatistic>& spstat, const Mat_<float>& fgdEnergy,
const Mat_<float>& bgdEnergy, float gamma, Mat_<int>& label)
{
//::Graph<float,float,float> graph(nNode,nEdge,errfunc);
//graph
int nEdge = UtilsSuperpixel::CountEdge(spstat);
Mat_<int> edgeVertex(nEdge,2);
Mat_<float> edgeWeight(nEdge,1);
Mat_<float> edgeLen(nEdge,1);
int idx = 0;
for(int i=0;i<spstat.size();i++)
{
SuperpixelStatistic& sp = spstat[i];
for( set<int>::iterator j=sp.conn.begin();
j!= sp.conn.end();
j++)
{
int d = (*j);
SuperpixelStatistic& dsp = spstat[d];
if ( i != d)
{
edgeVertex(idx,0) = min(i,d);
edgeVertex(idx,1) = max(i,d);
float diff = (float) norm(sp.mean_color_ - dsp.mean_color_);
edgeWeight(idx) = diff*diff;
edgeLen(idx) = (float) cv::norm(sp.mean_position_-dsp.mean_position_);
idx++;
}
}
}
float beta = (float) cv::mean(edgeWeight)[0];
Graph<float,float,float> graph((int)spstat.size(), nEdge, errfunc);
graph.add_node((int)spstat.size());
for(int i=0;i<fgdEnergy.total();i++)
{
graph.add_tweights(i,bgdEnergy(i),fgdEnergy(i));
}
edgeWeight = - edgeWeight / beta;
cv::exp(edgeWeight,edgeWeight);
edgeWeight *= gamma;
cv::divide(edgeWeight, edgeLen,edgeWeight);
for(int i=0;i<nEdge;i++)
{
float w = edgeWeight(i);
graph.add_edge(edgeVertex(i,0),edgeVertex(i,1),w,w);
}
graph.maxflow();
label.create((int)spstat.size(),1);
for(int i=0;i<spstat.size();i++)
{
if ( graph.what_segment(i) == Graph<float,float,float>::SOURCE)
{
label(i) = 1;
}
else
{
label(i) = 0;
}
}
}
bool c_FourierTransfrom::ifftw_complex_3d(const Mat_<Vec6d> &_input,
Mat_<Vec6d> &_output)
{
size_t height = _input.rows;
size_t width = _input.cols;
size_t n_channels = _input.channels() / 2;
size_t n_pixels = height * width;
size_t n_data = n_pixels * n_channels;
fftw_complex *in, *out;
fftw_plan p;
in = (fftw_complex *)fftw_malloc(sizeof(fftw_complex) * n_data);
out = (fftw_complex *)fftw_malloc(sizeof(fftw_complex) * n_data);
p = fftw_plan_dft_3d(height, width, n_channels, in, out, FFTW_BACKWARD,
FFTW_ESTIMATE);
/*!< prepare the data */
for (size_t i_row = 0; i_row < height; ++i_row)
{
const Vec3d *p = _input.ptr<Vec3d>(i_row);
for (size_t i_col = 0; i_col < width; ++i_col)
{
size_t index = i_row * width + i_col;
for (size_t k = 0; k < n_channels; ++k)
{
in[n_pixels * k + index][0] = p[i_col][k];
in[n_pixels * k + index][1] = p[i_col][k + n_channels];
}
#if 0
in[index][0] = p[i_col][4];
in[index][1] = p[i_col][5];
in[n_pixels + index][0] = p[i_col][2];
in[n_pixels + index][1] = p[i_col][3];
in[n_pixels * 2 + index][0] = p[i_col][0];
in[n_pixels * 2 + index][1] = p[i_col][1];
#endif
}
}
fftw_execute(p);
/*!< write back data */
_output = Mat_<Vec6d>::zeros(_input.size());
for (size_t i_row = 0; i_row < height; ++i_row)
{
Vec6d *p = _output.ptr<Vec6d>(i_row);
for (size_t i_col = 0; i_col < width; ++i_col)
{
size_t index = i_row * width + i_col;
for (size_t k = 0; k < n_channels; ++k)
{
p[i_col][k] = out[n_pixels * k + index][0];
p[i_col][k + n_channels] = out[n_pixels * k + index][1];
}
#if 0
p[i_col][0] = out[n_pixels * 2 + index][0];
p[i_col][1] = out[n_pixels + index][0];
p[i_col][2] = out[index][0];
p[i_col][3] = out[n_pixels * 2 + index][1];
p[i_col][4] = out[n_pixels + index][1];
p[i_col][5] = out[index][1];
#endif
}
}
_output /= n_data;
fftw_destroy_plan(p);
fftw_free(in);
fftw_free(out);
return true;
}
void collectData(int subjId,
CascadeClassifier &classifier,
ShapePredictor &predictor,
Mat_<float> &labels,
Mat_<float> &multihog,
Mat_<float> &landmarks)
{
int H[] = { -15, -10, -5, 0, 5, 10, 15 };
int V[] = { -10, 0, 10 };
string path = to_string(subjId) + "/";
if (subjId < 10) path = "columbia/000" + path;
else path = "columbia/00" + path;
ifstream fin(path + "annotation.txt");
for (int imgId = 0; imgId < 105; imgId++) {
int p, v, h;
fin >> p >> v >> h;
if (abs(p) > 15) continue;
string imgpath = path + to_string(imgId) + ".jpg";
Mat_<uchar> img = imread(imgpath, 0);
BBox bbox = getTestBBox(img, classifier);
if (EmptyBox(bbox)) continue;
int l = 0;
// EYE, MOUTH, NOF
if (abs(h) <= 5 && v == 0) l = 0;
else if (abs(h) <= 5 && v == -10) l = 1;
else l = 2;
if (l == 2) {
RNG rng(getTickCount());
double num = rng.uniform(0.0, 1.0);
if (num > 0.5) continue;
}
// 上中下
/*if (v < 0) l = 0;
else if (v == 0) l = 1;
else l = 2;*/
// 9分类
/*if (h < -5) l += 0;
else if (h > 5) l += 2;
else l += 1;
if (v < 0) l += 0;
else if (v > 0) l += 2 * 3;
else l += 1 * 3;*/
Mat_<float> lab = l*Mat_<float>::ones(1, 1);
labels.push_back(lab);
Mat_<double> shape = predictor(img, bbox);
Geom G; initGeom(shape, G);
Pose P; calcPose(G, P);
Mat_<uchar> lEye, rEye;
regularize(img, bbox, P, shape, lEye, rEye);
vector<float> lRlt;
vector<float> rRlt;
calcMultiHog(lEye, lRlt);
calcMultiHog(rEye, rRlt);
vector<float> _hog2nd_vec;
for (int k = 0; k < lRlt.size(); k++)
_hog2nd_vec.push_back(lRlt[k]);
for (int k = 0; k < rRlt.size(); k++)
_hog2nd_vec.push_back(rRlt[k]);
Mat_<float> _hog2nd_row = Mat_<float>(_hog2nd_vec).reshape(1, 1);
multihog.push_back(_hog2nd_row);
vector<float> _ldmks;
for (int i = 28; i < 48; i++) {
_ldmks.push_back((shape(i, 0) - bbox.cx) / bbox.w);
_ldmks.push_back((shape(i, 1) - bbox.cy) / bbox.h);
}
float mouthx = (shape(51, 0) + shape(62, 0) + shape(66, 0) + shape(57, 0)) / 4;
float mouthy = (shape(51, 1) + shape(62, 1) + shape(66, 1) + shape(57, 1)) / 4;
_ldmks.push_back((mouthx - bbox.cx) / bbox.w);
_ldmks.push_back((mouthy - bbox.cy) / bbox.h);
float maxVal = *std::max_element(_ldmks.begin(), _ldmks.end());
for (int i = 0; i < _ldmks.size(); i++) _ldmks[i] *= 1.0 / maxVal; // scale to [-1, 1]
Mat_<float> ldmks = Mat_<float>(_ldmks).reshape(1, 1);
landmarks.push_back(ldmks);
}
fin.close();
}
void computePoseDifference(Mat img1, Mat img2, CommandArgs args, Mat k, Mat& dist_coefficients, double& worldScale, Mat& R, Mat& t, Mat& img_matches)
{
cout << "%===============================================%" << endl;
Mat camera_matrix = k.clone();
if (args.resize_factor > 1)
{
resize(img1, img1, Size(img1.cols / args.resize_factor,
img1.rows / args.resize_factor)); // make smaller for performance and displayablity
resize(img2, img2, Size(img2.cols / args.resize_factor,
img2.rows / args.resize_factor));
// scale matrix down according to changed resolution
camera_matrix = camera_matrix / args.resize_factor;
camera_matrix.at<double>(2,2) = 1;
}
Mat K1, K2;
K1 = K2 = camera_matrix;
if (img1.rows > img1.cols) // it is assumed the camera has been calibrated in landscape mode, so undistortion must also be performed in landscape orientation, or the camera matrix must be modified (fx,fy and cx,cy need to be exchanged)
{
swap(K1.at<double>(0,0), K1.at<double>(1,1));
swap(K1.at<double>(0,2), K1.at<double>(1,2));
}
if (img2.rows > img2.cols)
{
swap(K2.at<double>(0,0), K2.at<double>(1,1));
swap(K2.at<double>(0,2), K2.at<double>(1,2));
}
// Feature detection + extraction
vector<KeyPoint> KeyPoints_1, KeyPoints_2;
Mat descriptors_1, descriptors_2;
Ptr<Feature2D> feat_detector;
if (args.detector == DETECTOR_KAZE)
{
feat_detector = AKAZE::create(args.detector_data.upright ? AKAZE::DESCRIPTOR_MLDB_UPRIGHT : AKAZE::DESCRIPTOR_MLDB,
args.detector_data.descriptor_size,
args.detector_data.descriptor_channels,
args.detector_data.threshold,
args.detector_data.nOctaves,
args.detector_data.nOctaveLayersAkaze);
} else if (args.detector == DETECTOR_SURF)
{
feat_detector = xfeatures2d::SURF::create(args.detector_data.minHessian,
args.detector_data.nOctaves, args.detector_data.nOctaveLayersAkaze, args.detector_data.extended, args.detector_data.upright);
} else if (args.detector == DETECTOR_SIFT)
{
feat_detector = xfeatures2d::SIFT::create(args.detector_data.nFeatures,
args.detector_data.nOctaveLayersSift, args.detector_data.contrastThreshold, args.detector_data.sigma);
}
feat_detector->detectAndCompute(img1, noArray(), KeyPoints_1, descriptors_1);
feat_detector->detectAndCompute(img2, noArray(), KeyPoints_2, descriptors_2);
cout << "Number of feature points (img1, img2): " << "(" << KeyPoints_1.size() << ", " << KeyPoints_2.size() << ")" << endl;
// Find correspondences
BFMatcher matcher;
vector<DMatch> matches;
if (args.use_ratio_test)
{
if (args.detector == DETECTOR_KAZE)
matcher = BFMatcher(NORM_HAMMING, false);
else matcher = BFMatcher(NORM_L2, false);
vector<vector<DMatch>> match_candidates;
const float ratio = args.ratio;
matcher.knnMatch(descriptors_1, descriptors_2, match_candidates, 2);
for (int i = 0; i < match_candidates.size(); i++)
if (match_candidates[i][0].distance < ratio * match_candidates[i][1].distance)
matches.push_back(match_candidates[i][0]);
cout << "Number of matches passing ratio test: " << matches.size() << endl;
} else
{
if (args.detector == DETECTOR_KAZE)
matcher = BFMatcher(NORM_HAMMING, true);
else matcher = BFMatcher(NORM_L2, true);
matcher.match(descriptors_1, descriptors_2, matches);
cout << "Number of matching feature points: " << matches.size() << endl;
}
// Convert correspondences to vectors
vector<Point2f>imgpts1,imgpts2;
for(unsigned int i = 0; i < matches.size(); i++)
{
imgpts1.push_back(KeyPoints_1[matches[i].queryIdx].pt);
imgpts2.push_back(KeyPoints_2[matches[i].trainIdx].pt);
}
Mat mask; // inlier mask
if (args.undistort)
{
undistortPoints(imgpts1, imgpts1, K1, dist_coefficients, noArray(), K1);
undistortPoints(imgpts2, imgpts2, K2, dist_coefficients, noArray(), K2);
}
double focal = camera_matrix.at<double>(0,0);
Point2d principalPoint(camera_matrix.at<double>(0,2),camera_matrix.at<double>(1,2));
Mat E = findEssentialMat(imgpts1, imgpts2, focal, principalPoint, RANSAC, 0.999, 1, mask);
/* Mat F = camera_matrix.t().inv() * E * camera_matrix.inv(); */
Mat F = findFundamentalMat(imgpts1, imgpts2, CV_FM_RANSAC);
correctMatches(F, imgpts1, imgpts2, imgpts1, imgpts2);
cout << "Reprojection error:\n " << computeReprojectionError(imgpts1, imgpts2, mask, F) << endl;
int inliers = recoverPose(E, imgpts1, imgpts2, R, t, focal, principalPoint, mask);
cout << "Matches used for pose recovery:\n " << inliers << endl;
/* Mat R1, R2, ProjMat1, ProjMat2, Q; */
/* stereoRectify(camera_matrix, dist_coefficients, camera_matrix, dist_coefficients, img1.size(), R, t, R1, R2, ProjMat1, ProjMat2, Q); */
/* cout << "P1=" << ProjMat1 << endl; */
/* cout << "P2=" << ProjMat2 << endl; */
/* cout << "Q=" << Q << endl; */
Mat mtxR, mtxQ;
Mat Qx, Qy, Qz;
Vec3d angles = RQDecomp3x3(R, mtxR, mtxQ, Qx, Qy, Qz);
/* cout << "Qx:\n " << Qx << endl; */
/* cout << "Qy:\n " << Qy << endl; */
/* cout << "Qz:\n " << Qz << endl; */
cout << "Translation:\n " << t.t() << endl;
cout << "Euler angles [x y z] in degrees:\n " << angles.t() << endl;
if (args.epilines)
{
drawEpilines(Mat(imgpts1), 1, F, img2);
drawEpilines(Mat(imgpts2), 2, F, img1);
}
drawMatches(img1, KeyPoints_1, img2, KeyPoints_2, // draw only inliers given by mask
matches, img_matches, Scalar::all(-1), Scalar::all(-1), mask);
vector<Point2f> imgpts1_masked, imgpts2_masked;
for (int i = 0; i < imgpts1.size(); i++)
{
if (mask.at<uchar>(i,0) == 1)
{
imgpts1_masked.push_back(imgpts1[i]);
imgpts2_masked.push_back(imgpts2[i]);
}
}
Mat pnts4D;
Mat P1 = camera_matrix * Mat::eye(3, 4, CV_64FC1), P2;
Mat p2[2] = { R, t };
hconcat(p2, 2, P2);
P2 = camera_matrix * P2;
#define USE_OPENCV_TRIANGULATION
#ifndef USE_OPENCV_TRIANGULATION // strangely, both methods yield identical results
vector<Point3d> homogPoints1, homogPoints2;
for (int i = 0; i < imgpts1_masked.size(); i++)
{
Point2f currentPoint1 = imgpts1_masked[i];
homogPoints1.push_back(Point3d(currentPoint1.x, currentPoint1.y, 1));
Point2f currentPoint2 = imgpts2_masked[i];
homogPoints2.push_back(Point3d(currentPoint2.x, currentPoint2.y, 1));
}
Mat dehomogenized(imgpts1_masked.size(), 3, CV_64FC1);
for (int i = 0; i < imgpts1_masked.size(); i++)
{
Mat_<double> triangulatedPoint = IterativeLinearLSTriangulation(homogPoints1[i], P1, homogPoints2[i], P2);
Mat r = triangulatedPoint.t();
r.colRange(0,3).copyTo(dehomogenized.row(i)); // directly assigning to dehomogenized.row(i) compiles but does nothing, wtf?
}
#else
triangulatePoints(P1, P2, imgpts1_masked, imgpts2_masked, pnts4D);
pnts4D = pnts4D.t();
Mat dehomogenized;
convertPointsFromHomogeneous(pnts4D, dehomogenized);
dehomogenized = dehomogenized.reshape(1); // instead of 3 channels and 1 col, we want 1 channel and 3 cols
#endif
double mDist = 0;
int n = 0;
int pos = 0, neg = 0;
/* Write ply file header */
ofstream ply_file("points.ply", ios_base::trunc);
ply_file <<
"ply\n"
"format ascii 1.0\n"
"element vertex " << dehomogenized.rows << "\n"
"property float x\n"
"property float y\n"
"property float z\n"
"property uchar red\n"
"property uchar green\n"
"property uchar blue\n"
"end_header" << endl;
Mat_<double> row;
for (int i = 0; i < dehomogenized.rows; i++)
{
row = dehomogenized.row(i);
double d = row(2);
if (d > 0)
{
pos++;
mDist += norm(row);
n++;
/* float startx=imgpts1_masked[i].x - 1, starty=imgpts1_masked[i].y - 1, endx=imgpts1_masked[i].x + 1, endy=imgpts1_masked[i].y + 1; */
/* cout << "startx,endx = " << startx << "," << endx << endl; */
/* cout << "starty,endy = " << starty << "," << endy << endl; */
Vec3b rgb = img1.at<Vec3b>(imgpts1_masked[i].x, imgpts1_masked[i].y);
ply_file << row(0) << " " << row(1) << " " << row(2) << " " << (int)rgb[2] << " " << (int)rgb[1] << " " << (int)rgb[0] << "\n";
} else
{
neg++;
ply_file << 0 << " " << 0 << " " << 0 << " " << 0 << " " << 0 << " " << 0 << "\n";
}
}
ply_file.close();
mDist /= n;
worldScale = mDist;
cout << "Mean distance of " << n << " points to camera:\n " << mDist << " (dehomogenized)" << endl;
cout << "pos=" << pos << ", neg=" << neg << endl;
/* char filename[100]; */
/* sprintf(filename, "mat_1%d", i+1); */
/* Ptr<Formatter> formatter = Formatter::get(Formatter::FMT_CSV); */
/* Ptr<Formatted> formatted = formatter->format(dehomogenized); */
/* ofstream file(filename, ios_base::trunc); */
/* file << formatted << endl; */
/* Removed until cmake has been fathomed */
/* vector< Point3d > points3D; */
/* vector< vector< Point2d > > pointsImg; */
/* int NPOINTS=dehomogenized.rows; // number of 3d points */
/* int NCAMS=2; // number of cameras */
/* points3D.resize(NPOINTS); */
/* for (int i = 0; i < NPOINTS; i++) */
/* { */
/* points3D[i] = Point3d(dehomogenized.at<double>(i,0), */
/* dehomogenized.at<double>(i,1), */
/* dehomogenized.at<double>(i,2) */
/* ); */
/* } */
/* // fill image projections */
/* vector<vector<int> > visibility(2, vector<int>(NPOINTS, 1)); */
/* vector<Mat> camera_matrices(2, camera_matrix); */
/* vector<Mat> Rs(2); */
/* Rodrigues(Mat::eye(3, 3, CV_64FC1), Rs[0]); */
/* Rodrigues(R, Rs[0]); */
/* vector<Mat> Ts = { Mat::zeros(3,1, CV_64FC1), t }; */
/* vector<Mat> dist_coefficientss(2, dist_coefficients); */
/* pointsImg.resize(NCAMS); */
/* for(int i=0; i<NCAMS; i++) pointsImg[i].resize(NPOINTS); */
/* for (int i = 0; i < NPOINTS; i++) */
/* { */
/* pointsImg[0][i] = Point2d(imgpts1_masked[i].x, imgpts1_masked[i].y); */
/* pointsImg[1][i] = Point2d(imgpts2_masked[i].x, imgpts2_masked[i].y); */
/* } */
/* cvsba::Sba sba; */
/* sba.run(points3D, pointsImg, visibility, camera_matrices, Rs, Ts, dist_coefficientss); */
/* cout<<"Initial error="<<sba.getInitialReprjError()<<". "<< */
/* "Final error="<<sba.getFinalReprjError()<<endl; */
cout << "%===============================================%" << endl;
}
int main(int argc, char **argv)
{
ros::init(argc, argv, "rovioTest");
ros::NodeHandle n;
imageClient = n.serviceClient<rovio_base::image>("rovioImage");
controlClient = n.serviceClient<rovio_base::manDrv>("rovioControl");
reportClient = n.serviceClient<rovio_base::report>("rovioReport");
Mat imgOrd = getImage();
cvtColor(imgOrd, imgOrd, CV_RGB2GRAY);
const int AREA_SIZE = 16;
int ordRows = imgOrd.rows;
int ordCols = imgOrd.cols;
int areaNumY = ordRows / AREA_SIZE;
int areaNumX = ordCols / AREA_SIZE;
int marginY = ordRows % AREA_SIZE;
int areaNum = areaNumX * areaNumY;
Rect ior(0, marginY, areaNumX * AREA_SIZE, areaNumY * AREA_SIZE);
Mat img = getImg(imgOrd, ior);
VideoWriter videoWriter("/home/viki/Rovio.avi", CV_FOURCC('M', 'J', 'P', 'G'), 3.0, Size(img.cols, img.rows));
int lastDirection = 1;
for (int i = 0; i < 1000; i++)
{
Mat img = getImg(getImage(), ior);
Mat_<int> regionMap(img.size());
regionMap.setTo(0);
regionMap(regionMap.rows - 18, regionMap.cols / 2) = 1;
regionMap(regionMap.rows - 10, regionMap.cols / 2) = 1;
RegionGrowthAlg alg;
alg.calcRegionMap(img, regionMap);
// Detect forward
int fAreaWidth = img.cols - 200;
int fAreaHeight = img.rows / 4;
int fTopX = (img.cols - fAreaWidth) / 2;
int fTopY = img.rows - fAreaHeight;
int ignorePixels = 1000;
Mat_<int> fArea = regionMap(Rect(fTopX, fTopY, fAreaWidth, fAreaHeight));
int fAreaSum = 0;
for (int i = 0; i < fArea.rows; i++)
{
int* pRow = fArea.ptr<int>(i);
for (int j = 0; j < fArea.cols; j++)
{
if (pRow[j] == 0)
fAreaSum++;
}
}
bool flagForward = fAreaSum < ignorePixels;
// Detect left and right
int lrAreaWidth = 100;
int marginX = 0;
int lrAreaHeight = fAreaHeight;
int lrTopY = img.rows - lrAreaHeight;
int lTopX = marginX;
int rTopX = img.cols - marginX - lrAreaWidth;
int lrIgnorePixels = 1000;
Mat_<int> lArea = regionMap(Rect(lTopX, lrTopY, lrAreaWidth, lrAreaHeight));
Mat_<int> rArea = regionMap(Rect(rTopX, lrTopY, lrAreaWidth, lrAreaHeight));
int lAreaSum = 0;
int rAreaSum = 0;
for (int i = 0; i < lArea.rows; i++)
{
int* plRow = lArea.ptr<int>(i);
int* prRow = rArea.ptr<int>(i);
for (int j = 0; j < lArea.cols; j++)
{
if (plRow[j] == 0)
lAreaSum++;
if (prRow[j] == 0)
rAreaSum++;
}
}
bool flagLeft = lAreaSum < lrIgnorePixels;
bool flagRight = rAreaSum < lrIgnorePixels;
//fArea.setTo(2);
lArea.setTo(3);
rArea.setTo(4);
Utility util;
util.drawSegmentBorder(img, regionMap);
// Mark info
//标记
int leftSum = 0;
int rightSum = 0;
int loopi = img.rows;
int loopj = img.cols;
for (int i = 0; i < loopi; i++)
{
int* pLeftRow = regionMap.ptr<int>(i);
int* pRIghtRow = pLeftRow + loopj / 2;
int loop = loopj / 2;
for (int j = 0; j < loop; j++)
{
if (pLeftRow[j] > 0)
{
leftSum++;
}
if (pRIghtRow[j] > 0)
{
rightSum++;
}
}
}
Point pos(loopj / 2 - 150, loopi / 2);
std::stringstream ss;
string tmp;
ss << leftSum;
ss >> tmp;
putText(img, tmp, pos, FONT_HERSHEY_SIMPLEX, 0.6, Scalar(0, 255, 0));
pos.x = loopj / 2 + 100;
ss.str("");
ss.clear();
ss << rightSum;
ss >> tmp;
putText(img, tmp, pos, FONT_HERSHEY_SIMPLEX, 0.6, Scalar(0, 255, 0));
int textLen = 40;
pos.x = fArea.cols / 2 - textLen + fTopX;
pos.y = fArea.rows / 2 + fTopY;
ss.str("");
ss.clear();
ss << fAreaSum;
ss >> tmp;
putText(img, tmp, pos, FONT_HERSHEY_SIMPLEX, 0.6, Scalar(0, 50, 255));
pos.x = lArea.cols / 2 - textLen + lTopX;
pos.y = lArea.rows / 2 + lrTopY;
ss.str("");
ss.clear();
ss << lAreaSum;
ss >> tmp;
putText(img, tmp, pos, FONT_HERSHEY_SIMPLEX, 0.6, Scalar(0, 50, 255));
pos.x = rArea.cols / 2 - textLen + rTopX;
pos.y = rArea.rows / 2 + lrTopY;
ss.str("");
ss.clear();
ss << rAreaSum;
ss >> tmp;
putText(img, tmp, pos, FONT_HERSHEY_SIMPLEX, 0.6, Scalar(0, 50, 255));
//检测直线区域
int lineLen = 200;
int lineStartX = img.cols / 2 - lineLen / 2;
int lineEndX = img.cols / 2 + lineLen / 2;
int lineY = img.rows - 140;
Point lineStart(lineStartX, lineY);
Point lineEnd(lineEndX, lineY);
line(img, lineStart, lineEnd, Scalar(255, 20, 20));
int blockNum = 0;
int* pLineRow = regionMap.ptr<int>(lineY);
for (int j = lineStartX; j < lineEndX; j++)
{
if (pLineRow[j] == 0)
{
blockNum++;
}
}
bool isBlocked = blockNum > lineLen / 2;
//视频
//cvtColor(img, img, CV_GRAY2RGB);
imshow("", img);
waitKey(10);
videoWriter << img;
//控制
isBlocked = (!flagLeft && !flagRight) || isBlocked;
int waitTime = 1;
rvMCUReport rvMcu = getReport();
if (rvMcu.isIrOn && rvMcu.isDetectedBarrier)
isBlocked = true;
if (true)
{
if (isBlocked)
{
int maxDif = 5000;
if (leftSum - rightSum > maxDif)
{
lastDirection = -1;
}
else if (rightSum - leftSum > maxDif)
{
lastDirection = 1;
}
if (lastDirection == -1)
{
control(5, 8);
waitKey(waitTime);
}
else
{
control(6, 8);
waitKey(waitTime);
}
}
else if (flagForward)
{
control(1, 6);
}
else if (flagLeft)
{
control(3, 8);
}
else if (flagRight)
{
control(4, 8);
}
else
{
printf("Error control");
}
}
}
return 0;
}
// We are implementing SLIC here. I'm too lazy to enfoce the connectivity
Mat_< int > Superpixel::slic( const Mat_< Vec3f >& im ) const {
// Compute the spacing and grid size of the superpixels
double sp_area = 1.0 * im.cols * im.rows / K_;
int Kx = 0.5 + im.cols / sqrt( sp_area ), Ky = 0.5 + im.rows / sqrt( sp_area );
int K = Kx*Ky;
int win_sz = 1.0 * sqrt(sp_area) + 1;
// Initialize the seeds on a regular grid
std::vector< int64_t > cnt( K );
std::vector< Point2d > seedsd( K );
std::vector< Point > seeds( K );
for( int i=0,k=0; i<Kx; i++ )
for( int j=0; j<Ky; j++, k++ )
seeds[k] = Point( (i+0.5)*(im.cols-1)/Kx, (j+0.5)*(im.rows-1)/Ky );
// Run k-means
Mat_<float> dist( im.size() );
Mat_<int> label( im.size() );
for( int it=0; it<n_iter_; it++ ) {
// Assignment step
dist = std::numeric_limits<float>::max();
label = -1;
for( int k=0; k<K; k++ ) {
Vec3f c = im( seeds[k] );
for( int j=std::max(0,seeds[k].y-win_sz); j<im.rows && j<=seeds[k].y+win_sz; j++ )
for( int i=std::max(0,seeds[k].x-win_sz); i<im.cols && i<=seeds[k].x+win_sz; i++ ){
double d = (i-seeds[k].x) * (i-seeds[k].x) + (j-seeds[k].y) * (j-seeds[k].y);
double cd = ( im( j, i ) - c ).dot( im( j, i ) - c );
d += col_w_ * col_w_ * cd;
if( d < dist( j, i ) ) {
dist( j, i ) = d;
label( j, i ) = k;
}
}
}
// Update
for( int k=0; k<K; k++ ) {
seedsd[k] = Point2d(0,0);
cnt[k] = 0;
}
for( int j=0; j<im.rows; j++ )
for( int i=0; i<im.cols; i++ ) {
// Fix all the pixels we messed up!
if ( label( j, i ) < 0 ) {
// printf("Oops that wasn't very slick :(\n");
for( int k=0; k<K; k++ ){
Vec3f c = im( seeds[k] );
double d = (i-seeds[k].x) * (i-seeds[k].x) + (j-seeds[k].y) * (j-seeds[k].y);
double cd = ( im( j, i ) - c ).dot( im( j, i ) - c );
d += col_w_ * col_w_ * cd;
if( d < dist( j, i ) ) {
dist( j, i ) = d;
label( j, i ) = k;
}
}
}
seedsd[ label( j, i ) ] += Point2d( i, j );
cnt[ label( j, i ) ] += 1;
}
for( int k=0; k<K; k++ )
if (cnt[k] > 0)
seeds[k] = Point( 0.5 + seedsd[k].x / cnt[k], 0.5 + seedsd[k].y / cnt[k] );
}
return label;
}
int copyMakeBorder(/*const*/ Mat_<_Tp, chs>& src, Mat_<_Tp, chs>& dst, int top, int bottom, int left, int right, int borderType, const Scalar& value = Scalar())
{
FBC_Assert(top >= 0 && bottom >= 0 && left >= 0 && right >= 0);
if (src.isSubmatrix() && (borderType & BORDER_ISOLATED) == 0) {
Size wholeSize;
Point ofs;
src.locateROI(wholeSize, ofs);
int dtop = std::min(ofs.y, top);
int dbottom = std::min(wholeSize.height - src.rows - ofs.y, bottom);
int dleft = std::min(ofs.x, left);
int dright = std::min(wholeSize.width - src.cols - ofs.x, right);
src.adjustROI(dtop, dbottom, dleft, dright);
top -= dtop;
left -= dleft;
bottom -= dbottom;
right -= dright;
}
if (dst.empty() || dst.rows != (src.rows + top + bottom) || dst.cols != (src.cols + left + right)) {
dst.release();
dst = Mat_<_Tp, chs>(src.rows + top + bottom, src.cols + left + right);
}
if (top == 0 && left == 0 && bottom == 0 && right == 0) {
if (src.data != dst.data || src.step != dst.step)
src.copyTo(dst);
return 0;
}
borderType &= ~BORDER_ISOLATED;
if (borderType != BORDER_CONSTANT) {
copyMakeBorder_8u(src.ptr(), src.step, src.size(), dst.ptr(), dst.step, dst.size(), top, left, src.elemSize(), borderType);
} else {
int cn = src.channels, cn1 = cn;
AutoBuffer<double> buf(cn);
scalarToRawData<_Tp, chs>(value, buf, cn);
copyMakeConstBorder_8u(src.ptr(), src.step, src.size(), dst.ptr(), dst.step, dst.size(), top, left, (int)src.elemSize(), (uchar*)(double*)buf);
}
return 0;
}
Mat_< int > Superpixel::geodesicSegmentation( const Mat_< Vec3f >& im ) const {
//std::tr1::uniform_int<int> distribution(-2, 2);
//std::tr1::mt19937 engine; // Mersenne twister MT19937
//auto randint = std::tr1::bind(distribution, engine);
int s;
srand((unsigned)time(NULL));
s=rand()%4-2;
// Compute the spacing and grid size of the superpixels
double sp_area = 1.0 * im.cols * im.rows / K_;
int Kx = 0.5 + im.cols / sqrt( sp_area ), Ky = 0.5 + im.rows / sqrt( sp_area );
int K = Kx*Ky;
int win_sz = 1.0 * sqrt(sp_area) + 1;
// Initialize the seeds on a regular grid
std::vector< int64_t > cnt( K );
std::vector< Point2d > seedsd( K );
std::vector< Point > seeds( K );
for( int i=0,k=0; i<Kx; i++ )
for( int j=0; j<Ky; j++, k++ )
seeds[k] = Point( (i+0.5)*(im.cols-1)/Kx, (j+0.5)*(im.rows-1)/Ky ) + Point(rand()%4-2,rand()%4-2);/*Point( randint(), randint() );*/
Mat_<float> dx( im.size() ), dy( im.size() );
for( int j=0; j<im.rows; j++ )
for( int i=0; i<im.cols; i++ ) {
if (i)
dx(j,i-1) = col_w_*sqrt( (im(j,i)-im(j,i-1)).dot(im(j,i)-im(j,i-1)) ) + 1;
if (j)
dy(j-1,i) = col_w_*sqrt( (im(j-1,i)-im(j,i)).dot(im(j-1,i)-im(j,i)) ) + 1;
}
// Run k-means
Mat_<float> dist( im.size() );
Mat_<int> label( im.size() );
for( int it=0; it<n_iter_; it++ ) {
// Assignment step
dist = std::numeric_limits<float>::max();
label = -1;
for( int k=0; k<K; k++ ) {
dist( seeds[k] ) = 0;
label( seeds[k] ) = k;
}
for( int IT=0; IT<2; IT++ ){
for( int j=0; j<im.rows; j++ )
for( int i=0; i<im.cols; i++ ) {
if (i && dist(j,i-1) + dx(j,i-1) < dist(j,i)) {
dist(j,i) = dist(j,i-1) + dx(j,i-1);
label(j,i) = label(j,i-1);
}
if (j && dist(j-1,i) + dx(j-1,i) < dist(j,i)) {
dist(j,i) = dist(j-1,i) + dy(j-1,i);
label(j,i) = label(j-1,i);
}
}
for( int j=im.rows-1; j>=0; j-- )
for( int i=im.cols-1; i>=0; i-- ) {
if (i && dist(j,i) + dx(j,i-1) < dist(j,i-1)) {
dist(j,i-1) = dist(j,i) + dx(j,i-1);
label(j,i-1) = label(j,i);
}
if (j && dist(j,i) + dx(j-1,i) < dist(j-1,i)) {
dist(j-1,i) = dist(j,i) + dy(j-1,i);
label(j-1,i) = label(j,i);
}
}
}
for( int k=0; k<K; k++ ) {
Vec3f c = im( seeds[k] );
for( int j=std::max(0,seeds[k].y-win_sz); j<im.rows && j<=seeds[k].y+win_sz; j++ )
for( int i=std::max(0,seeds[k].x-win_sz); i<im.cols && i<=seeds[k].x+win_sz; i++ ){
double d = (i-seeds[k].x) * (i-seeds[k].x) + (j-seeds[k].y) * (j-seeds[k].y);
double cd = ( im( j, i ) - c ).dot( im( j, i ) - c );
d += col_w_ * col_w_ * cd;
if( d < dist( j, i ) ) {
dist( j, i ) = d;
label( j, i ) = k;
}
}
}
// Update
for( int k=0; k<K; k++ ) {
seedsd[k] = Point2d(0,0);
cnt[k] = 0;
}
for( int j=0; j<im.rows; j++ )
for( int i=0; i<im.cols; i++ ) {
// Fix all the pixels we messed up!
if ( label( j, i ) < 0 ) {
// printf("Oops that wasn't very slick :(\n");
for( int k=0; k<K; k++ ){
Vec3f c = im( seeds[k] );
double d = (i-seeds[k].x) * (i-seeds[k].x) + (j-seeds[k].y) * (j-seeds[k].y);
double cd = ( im( j, i ) - c ).dot( im( j, i ) - c );
d += col_w_ * col_w_ * cd;
if( d < dist( j, i ) ) {
dist( j, i ) = d;
label( j, i ) = k;
}
}
}
seedsd[ label( j, i ) ] += Point2d( i, j );
cnt[ label( j, i ) ] += 1;
}
for( int k=0; k<K; k++ )
if (cnt[k] > 0)
seeds[k] = Point( 0.5 + seedsd[k].x / cnt[k], 0.5 + seedsd[k].y / cnt[k] );
}
return label;
}
void FaceAnalyser::AddNextFrame(const cv::Mat& frame, const CLMTracker::CLM& clm_model, double timestamp_seconds, bool online, bool visualise)
{
// Check if a reset is needed first (TODO same person no reset)
//if(face_bounding_box.area() > 0)
//{
// Rect_<double> new_bounding_box = clm.GetBoundingBox();
// // If the box overlaps do not need a reset
// double intersection_area = (face_bounding_box & new_bounding_box).area();
// double union_area = face_bounding_box.area() + new_bounding_box.area() - 2 * intersection_area;
// // If the model is already tracking what we're detecting ignore the detection, this is determined by amount of overlap
// if( intersection_area/union_area < 0.5)
// {
// this->Reset();
// }
// face_bounding_box = new_bounding_box;
//}
//if(!clm.detection_success)
//{
// this->Reset();
//}
frames_tracking++;
// First align the face if tracking was successfull
if(clm_model.detection_success)
{
AlignFaceMask(aligned_face, frame, clm_model, triangulation, true, align_scale, align_width, align_height);
}
else
{
aligned_face = Mat(align_height, align_width, CV_8UC3);
aligned_face.setTo(0);
}
if(aligned_face.channels() == 3)
{
cvtColor(aligned_face, aligned_face_grayscale, CV_BGR2GRAY);
}
else
{
aligned_face_grayscale = aligned_face.clone();
}
// Extract HOG descriptor from the frame and convert it to a useable format
Mat_<double> hog_descriptor;
Extract_FHOG_descriptor(hog_descriptor, aligned_face, this->num_hog_rows, this->num_hog_cols);
// Store the descriptor
hog_desc_frame = hog_descriptor;
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);
// Only update the running median if predictions are not high
// That is don't update it when the face is expressive (just retrieve it)
bool update_median = true;
// TODO test if this would be useful or not
//if(!this->AU_predictions.empty())
//{
// for(size_t i = 0; i < this->AU_predictions.size(); ++i)
// {
// if(this->AU_predictions[i].second > 1)
// {
// update_median = false;
// break;
// }
// }
//}
update_median = update_median & clm_model.detection_success;
// A small speedup
if(frames_tracking % 2 == 1)
{
UpdateRunningMedian(this->hog_desc_hist[orientation_to_use], this->hog_hist_sum[orientation_to_use], this->hog_desc_median, hog_descriptor, update_median, this->num_bins_hog, this->min_val_hog, this->max_val_hog);
}
// Geom descriptor and its median
geom_descriptor_frame = clm_model.params_local.t();
if(!clm_model.detection_success)
{
geom_descriptor_frame.setTo(0);
}
// Stack with the actual feature point locations (without mean)
Mat_<double> locs = clm_model.pdm.princ_comp * geom_descriptor_frame.t();
cv::hconcat(locs.t(), geom_descriptor_frame.clone(), geom_descriptor_frame);
// A small speedup
if(frames_tracking % 2 == 1)
{
UpdateRunningMedian(this->geom_desc_hist, this->geom_hist_sum, this->geom_descriptor_median, geom_descriptor_frame, update_median, this->num_bins_geom, this->min_val_geom, this->max_val_geom);
}
// First convert the face image to double representation as a row vector
Mat_<uchar> aligned_face_cols(1, aligned_face.cols * aligned_face.rows * aligned_face.channels(), aligned_face.data, 1);
Mat_<double> aligned_face_cols_double;
aligned_face_cols.convertTo(aligned_face_cols_double, CV_64F);
// TODO get rid of this completely as it takes too long?
//UpdateRunningMedian(this->face_image_hist[orientation_to_use], this->face_image_hist_sum[orientation_to_use], this->face_image_median, aligned_face_cols_double, update_median, 256, 0, 255);
// Visualising the median HOG
if(visualise)
{
FaceAnalysis::Visualise_FHOG(hog_descriptor, num_hog_rows, num_hog_cols, hog_descriptor_visualisation);
}
// 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;
}
this->current_time_seconds = timestamp_seconds;
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);
}
void spm_bp::runspm_bp(cv::Mat_<cv::Vec2f>& flowResult)
{
clock_t start_disp, finish_disp;
start_disp = clock();
printf("==================================================\n");
printf("SPM-BP begins\n");
Mat_<Vec2f> label_k(height1 * width1, NUM_TOP_K);
Mat_<float> dcost_k(height1 * width1, NUM_TOP_K);
#ifdef _WIN32
printf("Running on Windows\n");
// Dummy code that speeds up program on windows considerably
// Save label
vector<vector<Vec2f> > label_saved(numOfSP1);
//label_saved.reserve(10000);
vector<vector<Mat_<float> > > dcost_saved(numOfSP1);
//dcost_saved.reserve(10000);
for (int i = 0; i < numOfSP1; i++) {
label_saved[i].reserve(100000);
dcost_saved[i].reserve(100000);
}
#endif
//initiate labels
init_label_super(label_k, dcost_k);
//MESSAGE 0:left, 1:right, 2:up, 3:down
Mat_<Vec<float, NUM_TOP_K> > message(height1 * width1, 4);
message.setTo(0);
//precompute smooth weight
float omega[256];
for (int i = 0; i < 256; ++i)
omega[i] = lambda_smooth * std::exp(-float(i) / 20);
// TODO: Change to Mat4f
Mat_<Vec4f> smoothWt(height1, width1);
smoothWt.setTo(lambda_smooth);
// # pragma omp parallel for
for (int i = 1; i < height1 - 1; ++i)
{
for (int j = 1; j < width1 - 1; ++j) {
const Vec3f &ref = im1f[i][j];
// TODO: Don't need abs here since norm >= 0
// smoothWt[i][j][0] = omega[int(abs(norm(ref - im1f[i][j - 1])))];
// smoothWt[i][j][1] = omega[int(abs(norm(ref - im1f[i][j + 1])))];
// smoothWt[i][j][2] = omega[int(abs(norm(ref - im1f[i - 1][j])))];
// smoothWt[i][j][3] = omega[int(abs(norm(ref - im1f[i + 1][j])))];
smoothWt[i][j][0] = omega[int(norm(ref - im1f[i][j - 1]))];
smoothWt[i][j][1] = omega[int(norm(ref - im1f[i][j + 1]))];
smoothWt[i][j][2] = omega[int(norm(ref - im1f[i - 1][j]))];
smoothWt[i][j][3] = omega[int(norm(ref - im1f[i + 1][j]))];
}
}
float dis_belief_l[NUM_TOP_K];
float dis_belief_r[NUM_TOP_K];
float dis_belief_u[NUM_TOP_K];
float dis_belief_d[NUM_TOP_K];
const int BUFSIZE = NUM_TOP_K * 50;
vector<Vec2f> vec_label(BUFSIZE), vec_label_nei(BUFSIZE);
vector<float> vec_mes_l(BUFSIZE),
vec_mes_r(BUFSIZE),
vec_mes_u(BUFSIZE),
vec_mes_d(BUFSIZE),
vec_belief(BUFSIZE),
vec_d_cost(BUFSIZE);
Mat_<float> DataCost_nei;
start_disp = clock();
double wall_timer = omp_get_wtime();
if (display)
Show_WTA_Flow(-1, label_k, dcost_k, message, flowResult);
int spBegin, spEnd, spStep;
for (int iter = 0; iter < iterNum; iter++) {
if (iter % 2) {
spBegin = numOfSP1 - 1, spEnd = -1, spStep = -1;
}
else {
spBegin = 0, spEnd = numOfSP1, spStep = 1;
}
for (int sp = spBegin; sp != spEnd; sp += spStep) {
int curSPP = superpixelsList1[sp][0];
Vec4i curRange = subRange1[curSPP];
int y = curRange[0];
int x = curRange[1];
int w = curRange[2] - curRange[0] + 1;
int h = curRange[3] - curRange[1] + 1;
// rand neighbor pixel and store
vec_label_nei.clear();
std::set<int>::iterator sIt;
std::set<int>& sAdj = spGraph1[iter % 2].adjList[sp];
for (sIt = sAdj.begin(); sIt != sAdj.end(); ++sIt) {
repPixels1[*sIt] = superpixelsList1[*sIt][rand() % superpixelsList1[*sIt].size()];
Vec2f test_label;
//vector<Vec2f> & cur_label_k = label_k[repPixels1[*sIt]];
for (int k = 0; k < NUM_TOP_K; k++) {
//test_label= cur_label_k[k];
test_label = label_k[repPixels1[*sIt]][k];
if (isNewLabel_PMF_PMBP(vec_label_nei, test_label) == -1)
vec_label_nei.push_back(test_label);
}
}
repPixels1[sp] = superpixelsList1[sp][rand() % superpixelsList1[sp].size()];
for (int k = 0; k < NUM_TOP_K; k++) {
Vec2f test_label = label_k[repPixels1[sp]][k];
float mag = min(disp_range_u, disp_range_v) / 8;
for (; mag >= (1.0 / upScale); mag /= 2.0) {
Vec2f test_label_random;
float tmpVerLabel = test_label[0] + ((float(rand()) / RAND_MAX) - 0.5) * 2.0 * mag;
float tmpHorLabel = test_label[1] + ((float(rand()) / RAND_MAX) - 0.5) * 2.0 * mag;
test_label_random[0] = floor(tmpVerLabel * upScale + 0.5) / upScale;
test_label_random[1] = floor(tmpHorLabel * upScale + 0.5) / upScale;
if (test_label_random[0] >= -disp_range_u && test_label_random[0] <= disp_range_u
&& test_label_random[1] >= -disp_range_v && test_label_random[1] <= disp_range_v
&& isNewLabel_PMF_PMBP(vec_label_nei, test_label_random) == -1) //here
{
vec_label_nei.push_back(test_label_random);
}
}
}
const int vec_size = vec_label_nei.size();
// DataCost_nei.release();
DataCost_nei.create(h, w * vec_size);
DataCost_nei.setTo(0);
#if USE_CLMF0_TO_AGGREGATE_COST
cv::Mat_<cv::Vec4b> leftCombinedCrossMap;
leftCombinedCrossMap.create(h, w);
subCrossMap1[sp].copyTo(leftCombinedCrossMap);
CFFilter cff;
#endif
// Compute matching costs for each candidate particles
// printf("Computing matching costs start\n");
#pragma omp parallel for num_threads(NTHREADS)
for (int i = 0; i < vec_size; ++i) {
int kx = i * w;
Mat_<float> rawCost;
getLocalDataCostPerlabel(sp, vec_label_nei[i], rawCost);
#if USE_CLMF0_TO_AGGREGATE_COST
cff.FastCLMF0FloatFilterPointer(leftCombinedCrossMap, rawCost, rawCost);
#endif
rawCost.copyTo(DataCost_nei(cv::Rect(kx, 0, w, h)));
}
//getLocalDataCost(sp, vec_label_nei, DataCost_nei);
// printf("Computing matching costs end\n");
Vec4i curRange_s = spRange1[curSPP];
// int spw = curRange_s[2] - curRange_s[0] + 1;
// int sph = curRange_s[3] - curRange_s[1] + 1;
int spy_s, spy_e, spx_s, spx_e, spx_step, spy_step;
spy_s = curRange_s[0];
spy_e = curRange_s[2] + 1;
spx_s = curRange_s[1];
spx_e = curRange_s[3] + 1;
spx_step = 1;
spy_step = 1;
if (iter % 4 == 1) {
spy_s = curRange_s[2];
spy_e = curRange_s[0] - 1;
spx_s = curRange_s[3];
spx_e = curRange_s[1] - 1;
spx_step = -1;
spy_step = -1;
}
else if (iter % 4 == 2) {
spy_s = curRange_s[2];
spy_e = curRange_s[0] - 1;
spy_step = -1;
spx_s = curRange_s[1];
spx_e = curRange_s[3] + 1;
spx_step = 1;
}
else if (iter % 4 == 3)
{
spy_s = curRange_s[0];
spy_e = curRange_s[2] + 1;
spy_step = 1;
spx_s = curRange_s[3];
spx_e = curRange_s[1] - 1;
spx_step = -1;
}
for (int bi = spx_s; bi != spx_e; bi = bi + spx_step)
for (int bj = spy_s; bj != spy_e; bj = bj + spy_step) {
int p1 = bi * width1 + bj;
int pl = bi * width1 + (bj - 1);
int pu = (bi - 1) * width1 + bj;
int pr = bi * width1 + (bj + 1);
int pd = (bi + 1) * width1 + bj;
//Compute disbelief: three incoming message + data_cost
for (int k = 0; k < NUM_TOP_K; ++k) {
if (bj != 0)
dis_belief_l[k] = message[pl][0][k] + message[pl][2][k] + message[pl][3][k] + dcost_k[pl][k];
if (bj != width1 - 1)
dis_belief_r[k] = message[pr][1][k] + message[pr][2][k] + message[pr][3][k] + dcost_k[pr][k];
if (bi != 0)
dis_belief_u[k] = message[pu][0][k] + message[pu][1][k] + message[pu][2][k] + dcost_k[pu][k];
if (bi != height1 - 1)
dis_belief_d[k] = message[pd][0][k] + message[pd][1][k] + message[pd][3][k] + dcost_k[pd][k];
}
vec_label.clear();
vec_mes_l.clear();
vec_mes_r.clear();
vec_mes_u.clear();
vec_mes_d.clear();
vec_belief.clear();
vec_d_cost.clear();
// Update and save messages with current reference pixel's labels
Vec4f wt_s = smoothWt[bi][bj];
for (int k = 0; k < NUM_TOP_K; ++k) {
Vec2f test_label = label_k[p1][k];
vec_label.push_back(test_label);
float dcost = dcost_k[p1][k];
vec_d_cost.push_back(dcost);
//start_disp = clock();
float _mes_l = 0, _mes_r = 0, _mes_u = 0, _mes_d = 0;
if (bj != 0) {
_mes_l = ComputeMes_PMBP_per_label(dis_belief_l, label_k, pl, test_label, wt_s[0], tau_s);
vec_mes_l.push_back(_mes_l);
}
if (bj != width1 - 1) {
_mes_r = ComputeMes_PMBP_per_label(dis_belief_r, label_k, pr, test_label, wt_s[1], tau_s);
vec_mes_r.push_back(_mes_r);
}
if (bi != 0) {
_mes_u = ComputeMes_PMBP_per_label(dis_belief_u, label_k, pu, test_label, wt_s[2], tau_s);
vec_mes_u.push_back(_mes_u);
}
if (bi != height1 - 1) {
_mes_d = ComputeMes_PMBP_per_label(dis_belief_d, label_k, pd, test_label, wt_s[3], tau_s);
vec_mes_d.push_back(_mes_d);
}
vec_belief.push_back(_mes_l + _mes_r + _mes_u + _mes_d + dcost);
}
//propagation + random search
int kx;
for (int test_id = 0; test_id < vec_label_nei.size(); ++test_id) {
Vec2f test_label = vec_label_nei[test_id];
//if(isNewLabel_PMF_PMBP(vec_label,test_label)==-1)
{
vec_label.push_back(test_label);
kx = test_id * w;
const Mat_<float>& local = DataCost_nei(cv::Rect(kx, 0, w, h));
float dcost = local[bi - x][bj - y];
vec_d_cost.push_back(dcost);
//start_disp = clock();
float _mes_l = 0, _mes_r = 0, _mes_u = 0, _mes_d = 0;
if (bj != 0) {
_mes_l = ComputeMes_PMBP_per_label(dis_belief_l, label_k, pl, test_label, wt_s[0], tau_s);
vec_mes_l.push_back(_mes_l);
}
if (bj != width1 - 1) {
_mes_r = ComputeMes_PMBP_per_label(dis_belief_r, label_k, pr, test_label, wt_s[1], tau_s);
vec_mes_r.push_back(_mes_r);
}
if (bi != 0) {
_mes_u = ComputeMes_PMBP_per_label(dis_belief_u, label_k, pu, test_label, wt_s[2], tau_s);
vec_mes_u.push_back(_mes_u);
}
if (bi != height1 - 1) {
_mes_d = ComputeMes_PMBP_per_label(dis_belief_d, label_k, pd, test_label, wt_s[3], tau_s);
vec_mes_d.push_back(_mes_d);
}
vec_belief.push_back(_mes_l + _mes_r + _mes_u + _mes_d + dcost);
}
}
Compute_top_k(vec_belief, vec_label, vec_mes_l, vec_mes_r, vec_mes_u, vec_mes_d, vec_d_cost, message, label_k, dcost_k, p1, NUM_TOP_K);
//cout<<label_k[p1][0]<<endl;
Message_normalization_PMF_PMBP(message, p1, NUM_TOP_K);
}
//cout<<"Message "<<clock()-start_disp<<endl;
// propagation end
} //superpixel scan end
if (display)
Show_WTA_Flow(iter, label_k, dcost_k, message, flowResult);
finish_disp = clock();
std::cout << " time on clock(): " << (double)(clock() - start_disp) / CLOCKS_PER_SEC
<< " time on wall: " << omp_get_wtime() - wall_timer << "\n";
} //iteration
Show_WTA_Flow(iterNum, label_k, dcost_k, message, flowResult);
printf("==================================================\n");
printf("SPM-BP finished\n==================================================\n");
}
void CThinPlateSpline::computeSplineCoeffs(std::vector<Point>& iPIn, std::vector<Point>& iiPIn, float lambda,const TPS_INTERPOLATION tpsInter)
{
std::vector<Point>* iP = NULL;
std::vector<Point>* iiP = NULL;
if(tpsInter == FORWARD_WARP)
{
iP = &iPIn;
iiP = &iiPIn;
// keep information which coefficients are set
FLAG_COEFFS_BACK_WARP_SET = true;
FLAG_COEFFS_FORWARD_WARP_SET = false;
}
else if(tpsInter == BACK_WARP)
{
iP = &iiPIn;
iiP = &iPIn;
// keep information which coefficients are set
FLAG_COEFFS_BACK_WARP_SET = false;
FLAG_COEFFS_FORWARD_WARP_SET = true;
}
//get number of corresponding points
int dim = 2;
int n = iP->size();
//Initialize mathematical datastructures
Mat_<float> V(dim,n+dim+1,0.0);
Mat_<float> P(n,dim+1,1.0);
Mat_<float> K = (K.eye(n,n)*lambda);
Mat_<float> L(n+dim+1,n+dim+1,0.0);
// fill up K und P matrix
std::vector<Point>::iterator itY;
std::vector<Point>::iterator itX;
int y = 0;
for (itY = iP->begin(); itY != iP->end(); ++itY, y++) {
int x = 0;
for (itX = iP->begin(); itX != iP->end(); ++itX, x++) {
if (x != y) {
K(y, x) = (float)fktU(*itY, *itX);
}
}
P(y,1) = (float)itY->y;
P(y,2) = (float)itY->x;
}
Mat Pt;
transpose(P,Pt);
// insert K into L
Rect range = Rect(0, 0, n, n);
Mat Lr(L,range);
K.copyTo(Lr);
// insert P into L
range = Rect(n, 0, dim + 1, n);
Lr = Mat(L,range);
P.copyTo(Lr);
// insert Pt into L
range = Rect(0,n,n,dim+1);
Lr = Mat(L,range);
Pt.copyTo(Lr);
// fill V array
std::vector<Point>::iterator it;
int u = 0;
for(it = iiP->begin(); it != iiP->end(); ++it)
{
V(0,u) = (float)it->y;
V(1,u) = (float)it->x;
u++;
}
// transpose V
Mat Vt;
transpose(V,Vt);
cMatrix = Mat_<float>(n+dim+1,dim,0.0);
// invert L
Mat invL;
invert(L,invL,DECOMP_LU);
//multiply(invL,Vt,cMatrix);
cMatrix = invL * Vt;
//compensate for rounding errors
for(int row = 0; row < cMatrix.rows; row++)
{
for(int col = 0; col < cMatrix.cols; col++)
{
double v = cMatrix(row,col);
if(v > (-1.0e-006) && v < (1.0e-006) )
{
cMatrix(row,col) = 0.0;
}
}
}
}
void spm_bp::init_label_super(Mat_<Vec2f>& label_super_k, Mat_<float>& dCost_super_k) //, vector<vector<Vec2f> > &label_saved, vector<vector<Mat_<float> > > &dcost_saved)
{
printf("==================================================\n");
printf("Initiating particles...Done!\n");
vector<Vec2f> label_vec;
Mat_<float> localDataCost;
for (int sp = 0; sp < numOfSP1; ++sp) {
int id = repPixels1[sp];
int y = subRange1[id][0];
int x = subRange1[id][1];
int h = subRange1[id][3] - subRange1[id][1] + 1;
int w = subRange1[id][2] - subRange1[id][0] + 1;
label_vec.clear();
int k = 0;
while (k < NUM_TOP_K) {
float du = (float(rand()) / RAND_MAX - 0.5) * 2 * (float)disp_range_u;
float dv = (float(rand()) / RAND_MAX - 0.5) * 2 * (float)disp_range_v;
du = floor(du * upScale + 0.5) / upScale;
dv = floor(dv * upScale + 0.5) / upScale;
if (du >= -disp_range_u && du <= disp_range_u && dv >= -disp_range_v && dv <= disp_range_v) {
int index_tp = 1;
for (int k1 = 0; k1 < k; ++k1) {
if (checkEqual_PMF_PMBP(label_super_k[repPixels1[sp]][k1], Vec2f(du, dv)))
index_tp = 0;
}
if (index_tp == 1) {
for (int ii = 0; ii < superpixelsList1[sp].size(); ++ii)
label_super_k[superpixelsList1[sp][ii]][k] = Vec2f(du, dv);
label_vec.push_back(Vec2f(du, dv));
++k;
}
}
}
#if USE_CLMF0_TO_AGGREGATE_COST
cv::Mat_<cv::Vec4b> leftCombinedCrossMap;
leftCombinedCrossMap.create(h, w);
subCrossMap1[sp].copyTo(leftCombinedCrossMap);
CFFilter cff;
#endif
int vec_size = label_vec.size();
localDataCost.create(h, w * vec_size);
localDataCost.setTo(0);
#pragma omp parallel for num_threads(NTHREADS)
for (int i = 0; i < vec_size; ++i) {
int kx = i * w;
Mat_<float> rawCost;
getLocalDataCostPerlabel(sp, label_vec[i], rawCost);
#if USE_CLMF0_TO_AGGREGATE_COST
cff.FastCLMF0FloatFilterPointer(leftCombinedCrossMap, rawCost, rawCost);
#endif
rawCost.copyTo(localDataCost(cv::Rect(kx, 0, w, h)));
}
//getLocalDataCost( sp, label_vec, localDataCost);
int pt, px, py, kx;
for (int ii = 0; ii < superpixelsList1[sp].size(); ++ii) {
//cout<<ii<<endl;
pt = superpixelsList1[sp][ii];
px = pt / width1;
py = pt % width1;
for (int kk = 0; kk < NUM_TOP_K; kk++) {
kx = kk * w;
const Mat_<float>& local = localDataCost(cv::Rect(kx, 0, w, h));
dCost_super_k[pt][kk] = local[px - x][py - y];
}
}
}
printf("==================================================\n");
}
void visualize_3d_points(const Mat_<cv::Vec3f>& points_3d, std::string name) {
Mat points_3d_display;
points_3d.convertTo(points_3d_display, CV_8UC3, 100);
imshow(name, points_3d_display);
}
void spm_bp::getLocalDataCost(int sp, vector<Vec2f>& flowList, Mat_<float>& localDataCost) //, vector<vector<Vec2f> > &label_saved, vector<vector<Mat_<float> > > &dcost_saved)
{
int dSize = flowList.size();
//USE_COLOR_FEATURES
cv::Mat_<cv::Vec3f> subLt = subImage1[sp];
#if USE_CENSUS
vector<vector<bitset<CENSUS_SIZE_OF> > > subLt_css = subCensusBS1[sp];
//cv::Mat_<Vec_css> subLt_css = subCensus1[sp];
//cv::Mat_<Vec_css_bit> subLt_css_bit = subCensus_bit1[sp];
#endif
int upHeight, upWidth;
upHeight = im1Up.rows;
upWidth = im1Up.cols;
// extract sub-image from subrange
int p1 = repPixels1[sp];
int w = subRange1[p1][2] - subRange1[p1][0] + 1;
int h = subRange1[p1][3] - subRange1[p1][1] + 1;
int x = subRange1[p1][0];
int y = subRange1[p1][1];
#if USE_CLMF0_TO_AGGREGATE_COST
cv::Mat_<cv::Vec4b> leftCombinedCrossMap;
leftCombinedCrossMap.create(h, w);
subCrossMap1[sp].copyTo(leftCombinedCrossMap);
CFFilter cff;
#endif
Mat_<float> rawCost(h, w);
cv::Mat_<Vec3f> subRt(h, w);
cv::Mat_<Vec2f> subRt_g(h, w);
#if USE_CENSUS
vector<vector<bitset<CENSUS_SIZE_OF> > > subRt_css(h, vector<bitset<CENSUS_SIZE_OF> >(w));
#endif
cv::Mat_<float> tmpCost(h, w);
// localDataCost.release();
localDataCost.create(h, w * dSize);
int kx;
for (int kd = 0; kd < dSize; ++kd) {
kx = kd * w;
clock_t start = clock();
clock_t end;
//int check_id = isNewLabel_PMF_PMBP(label_saved[sp],flowList[kd]);
#if SAVE_DCOST
if (check_id >= 0) {
cout << "hit" << endl;
//dcost_saved[sp][check_id].copyTo(localDataCost(cv::Rect(kx, 0, w, h)));
//cout<<"hit"<<end-start<<endl;
//continue;
}
#endif
start = clock();
Vec2f fl = flowList[kd];
Vec3f* subLtPtr = (cv::Vec3f*)(subLt.ptr(0));
Vec3f* subRtPtr = (cv::Vec3f*)(subRt.ptr(0));
float* rawCostPtr = (float*)(rawCost.ptr(0));
int cy, cx, oy, ox;
oy = y;
for (cy = 0; cy < h; ++cy, ++oy) {
ox = x;
for (cx = 0; cx < w; ++cx, ++ox) {
int oyUp, oxUp;
oyUp = (oy + fl[0]) * upScale;
oxUp = (ox + fl[1]) * upScale;
if (oyUp < 0)
oyUp = 0;
if (oyUp >= upHeight)
oyUp = upHeight - 1;
if (oxUp < 0)
oxUp = 0;
if (oxUp >= upWidth)
oxUp = upWidth - 1;
#if USE_POINTER_WISE
*subRtPtr++ = im2Up[oyUp][oxUp];
#else
subRt[cy][cx] = im2Up[oyUp][oxUp];
#endif
#if USE_CENSUS
subRt_css[cy][cx] = censusBS2[oyUp][oxUp];
#endif
}
}
// calculate raw cost
subLtPtr = (cv::Vec3f*)(subLt.ptr(0));
subRtPtr = (cv::Vec3f*)(subRt.ptr(0));
rawCostPtr = (float*)(rawCost.ptr(0));
int iy, ix;
for (iy = 0; iy < h; ++iy) {
for (ix = 0; ix < w; ++ix) {
#if DATA_COST_ADCENSUS
bitset<CENSUS_SIZE_OF> tmpBS = subRt_css[iy][ix] ^ subLt_css[iy][ix];
#if USE_POINTER_WISE
float dist_c = fabs((float)(*subLtPtr)[0] - (*subRtPtr)[0])
+ fabs((float)(*subLtPtr)[1] - (*subRtPtr)[1])
+ fabs((float)(*subLtPtr++)[2] - (*subRtPtr++)[2]);
#else
float dist_c = std::abs(subLt[iy][ix][0] - subRt[iy][ix][0])
+ std::abs(subLt[iy][ix][1] - subRt[iy][ix][1])
+ std::abs(subLt[iy][ix][2] - subRt[iy][ix][2]);
#endif
float dist_css = expCensusDiffTable[tmpBS.count()];
//cout<<dist_c/3<<endl;
float dist_ce = expColorDiffTable[int(dist_c / 3)];
//cout<<dist_css<<"and "<<dist_c1<<endl;
//float dist_css = 1-std::exp( - float(HammingDis_bit(subLt_css_bit[iy][ix],subRt_css_bit[iy][ix]))/miu);
#if USE_POINTER_WISE
*rawCostPtr++ = 255 * (dist_css + dist_ce);
#else
rawCost[iy][ix] = 255 * (dist_css + dist_ce); //beta*min(dist_c/3,tau_c);//beta*min(dist_c/3,tau_c);//*255 + beta*min(dist_c/3,tau_c);
#endif
#endif
}
}
#if USE_CLMF0_TO_AGGREGATE_COST
cff.FastCLMF0FloatFilterPointer(leftCombinedCrossMap, rawCost, tmpCost);
//cout<<"filtering"<<clock()-end<<endl;
#endif
#if SAVE_DCOST
label_saved[sp].push_back(fl);
//dcost_saved[sp].push_back(tmpCost);
#endif
tmpCost.copyTo(localDataCost(cv::Rect(kx, 0, w, h)));
end = clock();
//cout<<"not hit"<<end-start<<endl;
}
//cout<<label_saved[sp].size()<<endl;
}
int main(int argc, const char * argv[]) {
//----------------------
// Open an OpenNI device
//----------------------
//TODO: You'll want to open an RGB camera stream here too (the one with wich you wish to register the depth)
cout << "Device opening ..." << endl;
VideoCapture capture;
capture.open( CAP_OPENNI );
if( !capture.isOpened() )
{
cout << "Can not open a capture object." << endl;
return -1;
}
// We don't want registration on, since we're going to do it ourselves.
// Some devices with RGB cameras can perform registration on device
bool modeRes=false;
modeRes = capture.set( CAP_PROP_OPENNI_REGISTRATION, 0 );
if (!modeRes) {
cout << "Can't disable registration. That's crazy!\n" << endl;
return -1;
}
// Display the current configuration
cout << "\nDepth generator output mode:" << endl <<
"FRAME_WIDTH " << capture.get( CAP_PROP_FRAME_WIDTH ) << endl <<
"FRAME_HEIGHT " << capture.get( CAP_PROP_FRAME_HEIGHT ) << endl <<
"FRAME_MAX_DEPTH " << capture.get( CAP_PROP_OPENNI_FRAME_MAX_DEPTH ) << " mm" << endl <<
"FPS " << capture.get( CAP_PROP_FPS ) << endl <<
"REGISTRATION " << capture.get( CAP_PROP_OPENNI_REGISTRATION ) << endl;
//---------------------------------------
// Specify camera properties and geometry
//--------------------------------------
//TODO: Fill in the values for your setup.
// Depth camera intrinsics
Matx33f unregisteredCameraMatrix = Matx33f::eye();
unregisteredCameraMatrix(0,0) = 570.0f;
unregisteredCameraMatrix(1,1) = 570.0f;
unregisteredCameraMatrix(0,2) = 320.0f-0.5f;
unregisteredCameraMatrix(1,2) = 240.0f-0.5f;
// NOTE: The depth distortion coefficients are currently not used by the Registration class.
Vec<float, 5> unregisteredDistCoeffs(0,0,0,0,0);
// RGB camera intrinsics
Matx33f registeredCameraMatrix = Matx33f::eye();
registeredCameraMatrix(0,0) = 570.0f;
registeredCameraMatrix(1,1) = 570.0f;
registeredCameraMatrix(0,2) = 320.0f-0.5f;
registeredCameraMatrix(1,2) = 240.0f-0.5f;
Vec<float, 5> registeredDistCoeffs(0,0,0,0,0);
Size2i registeredImagePlaneSize = Size2i(640, 480);
// The rigid body transformation between cameras.
// Used as: uv_rgb = K_rgb * [R | t] * z * inv(K_ir) * uv_ir
Matx44f registrationRbt = Matx44f::eye();
registrationRbt(0,3) = .04;
//------------------------------
// Create our registration class
//------------------------------
oc::Registration registration(unregisteredCameraMatrix,
unregisteredDistCoeffs,
registeredCameraMatrix,
registeredDistCoeffs,
registrationRbt);
for (;;) {
Mat_<uint16_t> depthMap;
if( !capture.grab() )
{
cout << "Can't grab depth." << endl;
return -1;
}
else
{
if( capture.retrieve( depthMap, CAP_OPENNI_DEPTH_MAP ) )
{
// Actually perform the registration
Mat_<uint16_t> registeredDepth;
bool performDilation = false;
registration.registerDepthToColor(depthMap,
registeredImagePlaneSize,
registeredDepth,
performDilation);
//Display the unregistered and registered depth
const float scaleFactor = 0.05f;
{
Mat_<uint8_t> show;
depthMap.convertTo( show, CV_8UC1, scaleFactor );
imshow( "depth map", show );
}
{
Mat_<uint8_t> show;
registeredDepth.convertTo( show, CV_8UC1, scaleFactor );
imshow( "registered map", show );
}
}
}
if( waitKey( 1 ) >= 0 )
break;
}
return 0;
}
void spm_bp::getLocalDataCostPerlabel(int sp, const Vec2f& fl, Mat_<float>& localDataCost)
{
//USE_COLOR_FEATURES
cv::Mat_<cv::Vec3f> subLt = subImage1[sp];
#if USE_CENSUS
vector<vector<bitset<CENSUS_SIZE_OF> > > subLt_css = subCensusBS1[sp];
#endif
int upHeight, upWidth;
upHeight = im1Up.rows;
upWidth = im1Up.cols;
// extract sub-image from subrange
int p1 = repPixels1[sp];
int w = subRange1[p1][2] - subRange1[p1][0] + 1;
int h = subRange1[p1][3] - subRange1[p1][1] + 1;
int x = subRange1[p1][0];
int y = subRange1[p1][1];
// localDataCost.release();
localDataCost.create(h, w);
//Mat_<float> rawCost(h, w);
Mat_<Vec3f> subRt(h, w);
vector<vector<bitset<CENSUS_SIZE_OF> > > subRt_css(h, vector<bitset<CENSUS_SIZE_OF> >(w));
#if SAVE_DCOST
if (check_id >= 0) {
cout << "hit" << endl;
}
#endif
Vec3f* subLtPtr = (cv::Vec3f*)(subLt.ptr(0));
Vec3f* subRtPtr = (cv::Vec3f*)(subRt.ptr(0));
float* rawCostPtr = (float*)(localDataCost.ptr(0));
cv::Vec3f *im2UpPtr = (cv::Vec3f*) im2Up.ptr(0);
int im2UpWidth = im2Up.cols;
int maxHeight = upHeight - 1;
int maxWidth = upWidth - 1;
int cy, cx, oy, ox;
float fl0 = fl[0], fl1 = fl[1];
#pragma omp parallel for
for (cy = 0; cy < h; ++cy) {
oy = y + cy;
int oyUp = (oy + fl0) * upScale;
if (oyUp < 0)
oyUp = 0;
if (oyUp >= upHeight)
oyUp = maxHeight;
for (cx = 0; cx < w; ++cx) {
ox = x + cx;
int oxUp = (ox + fl1) * upScale;
if (oxUp < 0)
oxUp = 0;
if (oxUp >= upWidth)
oxUp = maxWidth;
#if USE_POINTER_WISE
// *subRtPtr++ = im2Up[oyUp][oxUp];
subRtPtr[cy * w + cx] = im2UpPtr[oyUp * im2UpWidth + oxUp];
#else
subRt[cy][cx] = im2Up[oyUp][oxUp];
#endif
#if USE_CENSUS
subRt_css[cy][cx] = censusBS2[oyUp][oxUp];
#endif
}
}
// calculate raw cost
subLtPtr = (cv::Vec3f*)(subLt.ptr(0));
subRtPtr = (cv::Vec3f*)(subRt.ptr(0));
rawCostPtr = (float*)(localDataCost.ptr(0));
int iy, ix;
for (iy = 0; iy < h; ++iy) {
for (ix = 0; ix < w; ++ix) {
#if DATA_COST_ADCENSUS
bitset<CENSUS_SIZE_OF> tmpBS = subRt_css[iy][ix] ^ subLt_css[iy][ix];
#if USE_POINTER_WISE
float dist_c = fabsf((*subLtPtr)[0] - (*subRtPtr)[0])
+ fabsf((*subLtPtr)[1] - (*subRtPtr)[1])
+ fabsf((*subLtPtr)[2] - (*subRtPtr)[2]);
++subLtPtr;
++subRtPtr;
#else
float dist_c = std::abs(subLt[iy][ix][0] - subRt[iy][ix][0])
+ std::abs(subLt[iy][ix][1] - subRt[iy][ix][1])
+ std::abs(subLt[iy][ix][2] - subRt[iy][ix][2]);
#endif
float dist_css = expCensusDiffTable[tmpBS.count()];
float dist_ce = expColorDiffTable[int(dist_c / 3)];
#if USE_POINTER_WISE
*rawCostPtr++ = 255 * (dist_css + dist_ce);
#else
localDataCost[iy][ix] = 255 * (dist_css + dist_ce); //beta*min(dist_c/3,tau_c);//beta*min(dist_c/3,tau_c);//*255 + beta*min(dist_c/3,tau_c);
#endif
#endif
}
}
#if SAVE_DCOST
label_saved[sp].push_back(fl);
#endif
}
int main(int argc, char** argv)
{
/*Polynomial2 poly2;
poly2.kuu = -1;
poly2.kuv = 1;
poly2.kvv= -1;
poly2.ku = 0.25;
poly2.kv = 0.25;
poly2.k1 = 5;
CurveRasterizer<Polynomial2> raster(1, 1, -100, 100, poly2);
CurveRasterizer2<Polynomial2> raster2(1, 1, -100, 100, poly2);
auto tr0 = clock();
int x1 = 0;
int x2 = 0;
for (int i = 0; i < 10000000; i++)
{
raster.step();
x1 += raster.x;
}
auto tr1 = clock();
for (int i = 0; i < 10000000; i++)
{
raster2.step();
x2 += raster2.x;
}
auto tr2 = clock();
cout << "optimized " << double(tr1 - tr0) / CLOCKS_PER_SEC << endl;
cout << "simple " << double(tr2 - tr1) / CLOCKS_PER_SEC << endl;
cout << x1 << " " << x2 << endl;
return 0;*/
ifstream paramFile(argv[1]);
if (not paramFile.is_open())
{
cout << argv[1] << " : ERROR, file is not found" << endl;
return 0;
}
array<double, 6> params;
cout << "EU Camera model parameters :" << endl;
for (auto & p: params)
{
paramFile >> p;
cout << setw(15) << p;
}
cout << endl;
paramFile.ignore();
array<double, 6> cameraPose;
cout << "Camera pose wrt the robot :" << endl;
for (auto & e: cameraPose)
{
paramFile >> e;
cout << setw(15) << e;
}
cout << endl;
paramFile.ignore();
Transformation<double> TbaseCamera(cameraPose.data());
array<double, 6> planePose;
cout << "Plane pose :" << endl;
for (auto & e: cameraPose)
{
paramFile >> e;
cout << setw(15) << e;
}
cout << endl;
paramFile.ignore();
Transformation<double> TbasePlane(cameraPose.data());
StereoParameters stereoParams;
paramFile >> stereoParams.u0;
paramFile >> stereoParams.v0;
paramFile >> stereoParams.disparityMax;
paramFile >> stereoParams.blockSize;
paramFile.ignore();
string imageDir;
getline(paramFile, imageDir);
string imageInfo, imageName;
array<double, 6> robotPose1, robotPose2;
getline(paramFile, imageInfo);
istringstream imageStream(imageInfo);
imageStream >> imageName;
for (auto & x : robotPose1) imageStream >> x;
Mat8 img1 = imread(imageDir + imageName, 0);
int counter = 2;
while (getline(paramFile, imageInfo))
{
istringstream imageStream(imageInfo);
imageStream >> imageName;
for (auto & x : robotPose2) imageStream >> x;
Transformation<double> T01(robotPose1.data()), T02(robotPose2.data());
Transformation<double> TleftRight = T01.compose(TbaseCamera).inverseCompose(T02.compose(TbaseCamera));
Mat8 img2 = imread(imageDir + imageName, 0);
EnhancedStereo stereo(TleftRight, img1.cols, img1.rows, params.data(), params.data(), stereoParams);
cv::Mat_<uint8_t> res;
auto t2 = clock();
stereo.comuteStereo(img1, img2, res);
auto t3 = clock();
// cout << double(t3 - t2) / CLOCKS_PER_SEC << endl;
Mat_<float> distMat;
Mat_<float> planeMat;
stereo.computeDistance(distMat);
Transformation<double> T0Camera = T01.compose(TbaseCamera);
stereo.generatePlane(T0Camera.inverseCompose(TbasePlane), planeMat,
vector<Vector3d>{Vector3d(-0.1, -0.1, 0), Vector3d(-0.1 + 3 * 0.45, -0.1, 0),
Vector3d(-0.1 + 3 * 0.45, 0.5, 0), Vector3d(-0.1, 0.5, 0) } );
imshow("dist" + to_string(counter) , distMat);
imshow("plane" , planeMat);
imwrite("/home/bogdan/projects/plane.png", planeMat);
double err = 0;
double err2 = 0;
double dist = 0;
int N = 0;
int Nmax = 0;
Mat_<float> inlierMat(planeMat.size());
inlierMat.setTo(0);
for (int u = 0; u < distMat.cols; u++)
{
for (int v = 0; v < distMat.rows; v++)
{
if (planeMat(v, u) == 0) continue;
Nmax++;
dist += planeMat(v, u);
inlierMat(v, u) = 1;
if (distMat(v, u) == 0 or distMat(v, u) != distMat(v, u) or planeMat(v, u) != planeMat(v, u)) continue;
if (abs(distMat(v, u) - planeMat(v, u)) > 0.10) continue;
inlierMat(v, u) = 0;
err += distMat(v, u) - planeMat(v, u);
err2 += pow(distMat(v, u) - planeMat(v, u), 2);
N++;
}
}
// cout << (counter - 1) * 7 << " & " << dist/ Nmax * 1000 << " & " << err / N *1000 << " & " << sqrt(err2 / N)*1000
// << " & " << 100 * N / double(Nmax) << "\\\\" << endl << "\\hline" << endl;
cout << "avg err : " << err / N *1000 << " avg err2 : "
<< sqrt(err2 / N)*1000 << " number of inliers : " << 100 * N / double(Nmax) << endl;
imshow("diff" + to_string(counter), abs(planeMat - distMat));
imshow("inliers" + to_string(counter), inlierMat);
counter++;
}
waitKey();
return 0;
}
// Pick only the more stable/rigid points under changes of expression
void extract_rigid_points(Mat_<double>& source_points, Mat_<double>& destination_points)
{
if(source_points.rows == 68)
{
Mat_<double> tmp_source = source_points.clone();
source_points = 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));
Mat_<double> tmp_dest = destination_points.clone();
destination_points = 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));
}
}
