float CalculateError(const Mat_<float>& ground_truth_shape, const Mat_<float>& predicted_shape) {
Mat_<float> temp;
//temp = ground_truth_shape.rowRange(36, 37)-ground_truth_shape.rowRange(45, 46);
temp = ground_truth_shape.rowRange(1, 2) - ground_truth_shape.rowRange(6, 7);
float x = mean(temp.col(0))[0];
float y = mean(temp.col(1))[1];
float interocular_distance = sqrt(x*x + y*y);
float sum = 0;
for (int i = 0; i < ground_truth_shape.rows; i++) {
sum += norm(ground_truth_shape.row(i) - predicted_shape.row(i));
}
return sum / (ground_truth_shape.rows*interocular_distance);
}
Mat_<double> estimatePose(Mat_<double> const imagePts)
{
// Note that given R, t, the camera location in world
// coordinates is not just t, but instead -inv(R)*t.
Mat_<double> const worldPts = getWorldPts();
Mat_<double> const rotTransl = estimateRotTransl(worldPts, imagePts);
Mat_<double> rotMatrix(3, 3);
Mat_<double> translation(3, 1);
rotTransl.col(0).copyTo(rotMatrix.col(0));
rotTransl.col(1).copyTo(rotMatrix.col(1));
rotTransl.col(2).copyTo(rotMatrix.col(2));
rotTransl.col(3).copyTo(translation);
Mat_<double> cameraLoc = -rotMatrix.t() * translation;
Mat_<double> simplePose(4, 1);
simplePose(0) = cameraLoc(0); // x
simplePose(1) = cameraLoc(1); // y
simplePose(2) = cameraLoc(2); // z
// Yaw (rotation around z axis):
// See http://planning.cs.uiuc.edu/node103.html
simplePose(3) = atan2(rotMatrix(1, 0), rotMatrix(0, 0));
// cout << "simplePose: " << simplePose << endl;
return simplePose;
}
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);
}
// Need to move this all to opengl
void DrawBox(Mat image, Vec6d pose, Scalar color, int thickness, float fx, float fy, float cx, float cy)
{
float boxVerts[] = {-1, 1, -1,
1, 1, -1,
1, 1, 1,
-1, 1, 1,
1, -1, 1,
1, -1, -1,
-1, -1, -1,
-1, -1, 1};
Mat_<float> box = Mat(8, 3, CV_32F, boxVerts).clone() * 100;
Matx33f rot = Euler2RotMat(Vec3d(pose[3], pose[4], pose[5]));
Mat_<float> rotBox;
Mat((Mat(rot) * box.t())).copyTo(rotBox);
rotBox = rotBox.t();
rotBox.col(0) = rotBox.col(0) + pose[0];
rotBox.col(1) = rotBox.col(1) + pose[1];
rotBox.col(2) = rotBox.col(2) + pose[2];
// draw the lines
Mat_<float> rotBoxProj;
Project(rotBoxProj, rotBox, image.size(), fx, fy, cx, cy);
Mat begin;
Mat end;
rotBoxProj.row(0).copyTo(begin);
rotBoxProj.row(1).copyTo(end);
cv::line(image, Point((int)begin.at<float>(0), (int)begin.at<float>(1)), Point((int)end.at<float>(0), (int)end.at<float>(1)), color, thickness);
rotBoxProj.row(1).copyTo(begin);
rotBoxProj.row(2).copyTo(end);
cv::line(image, Point((int)begin.at<float>(0), (int)begin.at<float>(1)), Point((int)end.at<float>(0), (int)end.at<float>(1)), color, thickness);
rotBoxProj.row(2).copyTo(begin);
rotBoxProj.row(3).copyTo(end);
cv::line(image, Point((int)begin.at<float>(0), (int)begin.at<float>(1)), Point((int)end.at<float>(0), (int)end.at<float>(1)), color, thickness);
rotBoxProj.row(0).copyTo(begin);
rotBoxProj.row(3).copyTo(end);
//std::cout << begin <<endl;
//std::cout << end <<endl;
cv::line(image, Point((int)begin.at<float>(0), (int)begin.at<float>(1)), Point((int)end.at<float>(0), (int)end.at<float>(1)), color, thickness);
rotBoxProj.row(2).copyTo(begin);
rotBoxProj.row(4).copyTo(end);
cv::line(image, Point((int)begin.at<float>(0), (int)begin.at<float>(1)), Point((int)end.at<float>(0), (int)end.at<float>(1)), color, thickness);
rotBoxProj.row(1).copyTo(begin);
rotBoxProj.row(5).copyTo(end);
cv::line(image, Point((int)begin.at<float>(0), (int)begin.at<float>(1)), Point((int)end.at<float>(0), (int)end.at<float>(1)), color, thickness);
rotBoxProj.row(0).copyTo(begin);
rotBoxProj.row(6).copyTo(end);
cv::line(image, Point((int)begin.at<float>(0), (int)begin.at<float>(1)), Point((int)end.at<float>(0), (int)end.at<float>(1)), color, thickness);
rotBoxProj.row(3).copyTo(begin);
rotBoxProj.row(7).copyTo(end);
cv::line(image, Point((int)begin.at<float>(0), (int)begin.at<float>(1)), Point((int)end.at<float>(0), (int)end.at<float>(1)), color, thickness);
rotBoxProj.row(6).copyTo(begin);
rotBoxProj.row(5).copyTo(end);
cv::line(image, Point((int)begin.at<float>(0), (int)begin.at<float>(1)), Point((int)end.at<float>(0), (int)end.at<float>(1)), color, thickness);
rotBoxProj.row(5).copyTo(begin);
rotBoxProj.row(4).copyTo(end);
cv::line(image, Point((int)begin.at<float>(0), (int)begin.at<float>(1)), Point((int)end.at<float>(0), (int)end.at<float>(1)), color, thickness);
rotBoxProj.row(4).copyTo(begin);
rotBoxProj.row(7).copyTo(end);
cv::line(image, Point((int)begin.at<float>(0), (int)begin.at<float>(1)), Point((int)end.at<float>(0), (int)end.at<float>(1)), color, thickness);
rotBoxProj.row(7).copyTo(begin);
rotBoxProj.row(6).copyTo(end);
cv::line(image, Point((int)begin.at<float>(0), (int)begin.at<float>(1)), Point((int)end.at<float>(0), (int)end.at<float>(1)), color, thickness);
}
void Tree::Splitnode(const vector<Mat_<uchar> >& images,
const vector<Mat_<double> >& ground_truth_shapes,
const vector<Mat_<double> >& current_shapes,
const vector<BoundingBox> & bounding_box,
const Mat_<double>& mean_shape,
const Mat_<double>& shapes_residual,
const vector<int> &ind_samples,
// output
double& thresh,
double* feat,
bool& isvaild,
vector<int>& lcind,
vector<int>& rcind
){
if (ind_samples.size() == 0){
thresh = 0;
feat = new double[4];
feat[0] = 0;
feat[1] = 0;
feat[2] = 0;
feat[3] = 0;
lcind.clear();
rcind.clear();
isvaild = 1;
return;
}
// get candidate pixel locations
static int randomseedforfeature = 0;
RNG random_generator(getTickCount());
Mat_<double> candidate_pixel_locations(max_numfeats_,4);
for(unsigned int i = 0;i < max_numfeats_;i++){
double x1 = random_generator.uniform(-1.0,1.0);
double y1 = random_generator.uniform(-1.0,1.0);
double x2 = random_generator.uniform(-1.0,1.0);
double y2 = random_generator.uniform(-1.0,1.0);
//伪随机数
//double x1 = -1.0 + randomseedforfeature * 17%57 * (2.0/57);
//randomseedforfeature++;
//double y1 = -1.0 + randomseedforfeature * 17%57 * (2.0/57);
//randomseedforfeature++;
//double x2 = -1.0 + randomseedforfeature * 17%57 * (2.0/57);
//randomseedforfeature++;
//double y2 = -1.0 + randomseedforfeature * 17%57 * (2.0/57);
//randomseedforfeature++;
if((x1*x1 + y1*y1 > 1.0)||(x2*x2 + y2*y2 > 1.0)){
i--;
continue;
}
// cout << x1 << " "<<y1 <<" "<< x2<<" "<< y2<<endl;
candidate_pixel_locations(i,0) = x1 * max_radio_radius_;
candidate_pixel_locations(i,1) = y1 * max_radio_radius_;
candidate_pixel_locations(i,2) = x2 * max_radio_radius_;
candidate_pixel_locations(i,3) = y2 * max_radio_radius_;
}
// get pixel difference feature
Mat_<int> densities(max_numfeats_,(int)ind_samples.size());
for (int i = 0;i < ind_samples.size();i++){
Mat_<double> rotation;
double scaleX, scaleY;
Mat_<double> temp = ProjectShape(current_shapes[ind_samples[i]],bounding_box[ind_samples[i]]);
SimilarityTransform(temp,mean_shape,rotation,scaleX, scaleY);
// whether transpose or not ????
for(int j = 0;j < max_numfeats_;j++){
//some problems??? Ming
/*fixed bug, by Ming, 2015.08.20*/
//原来代码似乎没错.. 先改回去 2015.12.09, Ming
double project_x1 = rotation(0,0) * candidate_pixel_locations(j,0) + rotation(0,1) * candidate_pixel_locations(j,1);
double project_y1 = rotation(1,0) * candidate_pixel_locations(j,0) + rotation(1,1) * candidate_pixel_locations(j,1);
//double project_x1 = rotation(0,0) * candidate_pixel_locations(j,0) + rotation(1,0) * candidate_pixel_locations(j,1);
//double project_y1 = rotation(0,1) * candidate_pixel_locations(j,0) + rotation(1,1) * candidate_pixel_locations(j,1);
project_x1 = scaleX * project_x1 * bounding_box[ind_samples[i]].width / 2.0;
project_y1 = scaleY * project_y1 * bounding_box[ind_samples[i]].height / 2.0;
int real_x1 = project_x1 + current_shapes[ind_samples[i]](landmarkID_,0);
int real_y1 = project_y1 + current_shapes[ind_samples[i]](landmarkID_,1);
real_x1 = max(0.0,min((double)real_x1,images[ind_samples[i]].cols-1.0));
real_y1 = max(0.0,min((double)real_y1,images[ind_samples[i]].rows-1.0));
/*fixed bug, by Ming, 2015.08.20*/
//原来代码似乎没错.. 先改回去 2015.12.09, Ming
double project_x2 = rotation(0,0) * candidate_pixel_locations(j,2) + rotation(0,1) * candidate_pixel_locations(j,3);
double project_y2 = rotation(1,0) * candidate_pixel_locations(j,2) + rotation(1,1) * candidate_pixel_locations(j,3);
//double project_x2 = rotation(0,0) * candidate_pixel_locations(j,2) + rotation(1,0) * candidate_pixel_locations(j,3);
//double project_y2 = rotation(0,1) * candidate_pixel_locations(j,2) + rotation(1,1) * candidate_pixel_locations(j,3);
project_x2 = scaleX * project_x2 * bounding_box[ind_samples[i]].width / 2.0;
project_y2 = scaleY * project_y2 * bounding_box[ind_samples[i]].height / 2.0;
int real_x2 = project_x2 + current_shapes[ind_samples[i]](landmarkID_,0);
int real_y2 = project_y2 + current_shapes[ind_samples[i]](landmarkID_,1);
real_x2 = max(0.0,min((double)real_x2,images[ind_samples[i]].cols-1.0));
real_y2 = max(0.0,min((double)real_y2,images[ind_samples[i]].rows-1.0));
densities(j,i) = ((int)(images[ind_samples[i]](real_y1,real_x1))-(int)(images[ind_samples[i]](real_y2,real_x2)));
}
}
// pick the feature
Mat_<int> densities_sorted = densities.clone();
cv::sort(densities, densities_sorted, CV_SORT_ASCENDING);
vector<double> lc1,lc2;
vector<double> rc1,rc2;
lc1.reserve(ind_samples.size());
rc1.reserve(ind_samples.size());
lc2.reserve(ind_samples.size());
rc2.reserve(ind_samples.size());
// double E_x_2 = mean(shapes_residual.col(0).mul(shapes_residual.col(0)))[0];
// double E_x = mean(shapes_residual.col(0))[0];
// double E_y_2 = mean(shapes_residual.col(1).mul(shapes_residual.col(1)))[0];
// double E_y = mean(shapes_residual.col(1))[0];
// double var_overall = ind_samples.size()*((E_x_2 - E_x*E_x) + (E_y_2 - E_y*E_y));
double var_overall =(calculate_var(shapes_residual.col(0))+calculate_var(shapes_residual.col(1))) * ind_samples.size();
double max_var_reductions = 0;
double threshold = 0;
double var_lc = 0;
double var_rc = 0;
double var_reduce = 0;
double max_id = 0;
int thresholdTempNum = 10;
for (int i = 0; i <max_numfeats_;i++){
double bestReduce = -999999;
double bestTempThresd = 0;
for(int k=0; k<thresholdTempNum; ++k)
{
lc1.clear();
lc2.clear();
rc1.clear();
rc2.clear();
int ind =(int)(ind_samples.size() * random_generator.uniform(0.05,0.95));
//int ind =(int)(ind_samples.size() * 0.05 + k*0.9/thresholdTempNum);
//printf("%d th numfeats | %d th img sample \n",i,ind);
threshold = densities_sorted(i,ind);
for (int j=0;j < ind_samples.size();j++){
if (densities(i,j) < threshold){
lc1.push_back(shapes_residual(j,0));
lc2.push_back(shapes_residual(j,1));
}
else{
rc1.push_back(shapes_residual(j,0));
rc2.push_back(shapes_residual(j,1)); }
}
var_lc = (calculate_var(lc1)+calculate_var(lc2)) * lc1.size();
var_rc = (calculate_var(rc1)+calculate_var(rc2)) * rc1.size();
var_reduce = var_overall - var_lc - var_rc;
if(var_reduce > bestReduce)
{
bestReduce = var_reduce;
bestTempThresd = threshold;
}
}
//printf("var_reduce : %lf\n",var_reduce);
// cout << var_reduce<<endl;
if (bestReduce > max_var_reductions){
max_var_reductions = bestReduce;
thresh = bestTempThresd;
max_id = i;
}
}
//printf("max_var_reductions : %lf\n",max_var_reductions);
isvaild = 1;
//feat[0] =candidate_pixel_locations(max_id,0)/max_radio_radius_;
//feat[1] =candidate_pixel_locations(max_id,1)/max_radio_radius_;
//feat[2] =candidate_pixel_locations(max_id,2)/max_radio_radius_;
//feat[3] =candidate_pixel_locations(max_id,3)/max_radio_radius_;
feat[0] =candidate_pixel_locations(max_id,0);
feat[1] =candidate_pixel_locations(max_id,1);
feat[2] =candidate_pixel_locations(max_id,2);
feat[3] =candidate_pixel_locations(max_id,3);
// cout << max_id<< " "<<max_var_reductions <<endl;
// cout << feat[0] << " "<<feat[1] <<" "<< feat[2]<<" "<< feat[3]<<endl;
lcind.clear();
rcind.clear();
for (int j=0;j < ind_samples.size();j++){
if (densities(max_id,j) < thresh){
lcind.push_back(ind_samples[j]);
}
else{
rcind.push_back(ind_samples[j]);
}
}
}
Mat_<double> estimateRotTransl(
Mat_<double> const worldPts,
Mat_<double> const imagePts)
{
assert(imagePts.cols == 2);
assert(worldPts.cols == 3);
assert(imagePts.rows == worldPts.rows);
// TODO verify all worldPts have z=0
// See "pose estimation" section in the paper.
// Set up linear system of equations.
int const n = imagePts.rows;
Mat_<double> F(2 * n, 9);
for(int i = 0; i < n; i++)
{
F(2 * i, 0) = worldPts(i, 0);
F(2 * i, 1) = 0;
F(2 * i, 2) = -worldPts(i, 0) * imagePts(i, 0);
F(2 * i, 3) = worldPts(i, 1);
F(2 * i, 4) = 0;
F(2 * i, 5) = -worldPts(i, 1) * imagePts(i, 0);
F(2 * i, 6) = 1;
F(2 * i, 7) = 0;
F(2 * i, 8) = -imagePts(i, 0);
F(2 * i + 1, 0) = 0;
F(2 * i + 1, 1) = worldPts(i, 0);
F(2 * i + 1, 2) = -worldPts(i, 0) * imagePts(i, 1);
F(2 * i + 1, 3) = 0;
F(2 * i + 1, 4) = worldPts(i, 1);
F(2 * i + 1, 5) = -worldPts(i, 1) * imagePts(i, 1);
F(2 * i + 1, 6) = 0;
F(2 * i + 1, 7) = 1;
F(2 * i + 1, 8) = -imagePts(i, 1);
}
// Find least-squares estimate of rotation + translation.
SVD svd(F);
Mat_<double> rrp = svd.vt.row(8);
rrp = rrp.clone().reshape(0, 3).t();
if(rrp(2, 2) < 0) {
rrp *= -1; // make sure depth is positive
}
// cout << "rrp: " << rrp << endl;
Mat_<double> transl = \
2 * rrp.col(2) / (norm(rrp.col(0)) + norm(rrp.col(1)));
// cout << "transl: " << transl << endl;
Mat_<double> rot = Mat_<double>::zeros(3, 3);
rrp.col(0).copyTo(rot.col(0));
rrp.col(1).copyTo(rot.col(1));
SVD svd2(rot);
rot = svd2.u * svd2.vt;
if(determinant(rot) < 0) {
rot.col(2) *= -1; // make sure it's a valid rotation matrix
}
if(abs(determinant(rot) - 1) > 1e-10) {
cerr << "Warning: rotation matrix has determinant " \
<< determinant(rot) << " where expected 1." << endl;
}
// cout << "rot: " << rot << endl;
Mat_<double> rotTransl(3, 4);
rot.col(0).copyTo(rotTransl.col(0));
rot.col(1).copyTo(rotTransl.col(1));
rot.col(2).copyTo(rotTransl.col(2));
transl.copyTo(rotTransl.col(3));
return rotTransl;
}
