void HomographyModel::compute_residuals (const vector< ScoredMatch> &data_points, vector<float> &residuals) const { // residuals -> errors // Compute the residuals relative to the given parameter vector. // based on rrel_homography2d_est :: compute_residuals vnl_matrix< double > H(3,3); int r,c; for ( r=0; r<3; ++r ) for ( c=0; c<3; ++c ) H( r, c ) = parameters[ 3*r + c ]; vnl_svd< double > svd_H( H ); if ( svd_H.rank() < 3 ) { if (true) cout << "H == " << H << endl; //throw runtime_error("HomographyModel::compute_residuals rank(H) < 3!!"); cout << "HomographyModel::compute_residuals rank(H) < 3!!" << endl; } vnl_matrix< double > H_inv( svd_H.inverse() ); residuals.resize(data_points.size()); // compute the residual of each data point vector< ScoredMatch>::const_iterator data_points_it; vector<float>::iterator residuals_it; vnl_vector< double > from_point( 3 ), to_point( 3 ); vnl_vector< double > trans_pt( 3 ), inv_trans_pt( 3 ); double del_x, del_y, inv_del_x, inv_del_y; for (data_points_it = data_points.begin(), residuals_it = residuals.begin(); data_points_it != data_points.end() && residuals_it != residuals.end(); ++data_points_it, ++residuals_it) { // from feature a to feature b from_point[0] = data_points_it->feature_a->x; from_point[1] = data_points_it->feature_a->y; from_point[2] = 1.0; to_point[0] = data_points_it->feature_b->x; to_point[1] = data_points_it->feature_b->y; to_point[2] = 1.0; trans_pt = H * from_point; inv_trans_pt = H_inv * to_point; if ( from_point[ 2 ] == 0 || to_point[ 2 ] == 0 || trans_pt[ 2 ] == 0 || inv_trans_pt[ 2 ] == 0 ) { *residuals_it = 1e10; } else { del_x = trans_pt[ 0 ] / trans_pt[ 2 ] - to_point[ 0 ] /to_point[ 2 ]; del_y = trans_pt[ 1 ] / trans_pt[ 2 ] - to_point[ 1 ] / to_point[ 2 ]; inv_del_x = inv_trans_pt[ 0 ] / inv_trans_pt[ 2 ] -from_point[ 0 ] / from_point[ 2 ]; inv_del_y = inv_trans_pt[ 1 ] / inv_trans_pt[ 2 ] - from_point[ 1 ] / from_point[ 2 ]; const double t_value = vnl_math_sqr(del_x) + vnl_math_sqr(del_y) + vnl_math_sqr(inv_del_x) + vnl_math_sqr(inv_del_y); *residuals_it = vcl_sqrt( t_value ); } } // end of 'for each data point' return; } // end of method FundamentalMatrixModel<F>::compute_residuals
void CProxyCamera::GetGLMatrices(double glprojmatrix[], double glmodelviewmatrix[], const int glvpmatrix[], double znear, double zfar) const { double mat[3][4]; for (int i = 0; i < 3; ++i) { for (int j = 0; j < 3; ++j) { mat[i][j] = m_KR(i,j); } } mat[0][3] = m_KT[0]; mat[1][3] = m_KT[1]; mat[2][3] = m_KT[2]; Matrix3d LHC = Matrix3d::Zero(); LHC(0, 0) = LHC(1, 1) = LHC(2, 2) = -1; LHC(0, 0) = 1; double icpara[3][4], trans[3][4]; if (arParamDecompMat(mat, icpara, trans) < 0) { printf("Fatal error: proj decompose failed!\n"); exit(0); } Matrix3d R; for (int i = 0; i < 3; i++) { for (int j = 0; j < 3; j++) { R(i, j) = trans[i][j]; } } Matrix3d LHCR = LHC * R; Matrix4d modelViewMatrix = Matrix4d::Identity(); for (int i = 0; i < 3; i++) { for (int j = 0; j < 3; j++) { modelViewMatrix(i, j) = LHCR(i,j); } } modelViewMatrix(0, 3) = trans[0][3]; modelViewMatrix(1, 3) = trans[1][3]; modelViewMatrix(2, 3) = trans[2][3]; modelViewMatrix(1, 3) = modelViewMatrix(1, 3) * (-1); modelViewMatrix(2, 3) = modelViewMatrix(2, 3) * (-1); modelViewMatrix(3, 3) = 1.0; Matrix4d finalModelM=modelViewMatrix; for (int i=0;i<16;i++) { glmodelviewmatrix[i]=finalModelM(i); } double w = glvpmatrix[2]; double h = glvpmatrix[3]; Matrix4d H_inv = Matrix4d::Identity(); H_inv(0, 0) = 2.0 / w; H_inv(0, 2) = -1; H_inv(1, 1) = -2.0 / h; H_inv(1, 2) = 1.0; H_inv(3, 2) = 1.0; Matrix3d K = Matrix3d::Zero(); for (int i = 0; i < 3; i++) { for (int j = 0; j < 3; j++) { K(i, j) = icpara[i][j] / icpara[2][2]; } } Matrix3d y = K * LHC; Matrix4d y_ = Matrix4d::Zero(); for (int i = 0; i < 3; i++) { for (int j = 0; j < 3; j++) { y_(i, j) = y(i,j); } } Matrix4d result = H_inv * (y_); double C_ = -(zfar + znear) / (zfar - znear); double D_ = -(2 * zfar * znear) / (zfar - znear); result(2, 2) = C_; result(2, 3) = D_; Matrix4d finalRes=result; for (int i=0;i<16;i++) { glprojmatrix[i]=finalRes(i); } }
std::vector<BoundingBox> PedestrianDetector::generateCandidatesWCalibration(int imageHeight, int imageWidth, double *maxHeight, float meanHeight/* = 1.8m*/, float stdHeight/* = 0.1m*/, float factorStdHeight/* = 2.0*/) { // there is a set of parameters here that are hard coded, but should // be read from a file or something... cv::Mat_<float> P3 = P.col(2); //std::cout << "P3: " << P3 << std::endl; float aspectRatio = 0.5; float minImageHeight = 80; float stepHeight = 0.200; int totalCandidates = 0; std::vector<BoundingBox> all_candidates; double max_h = 0; // assemble the third line of the inverse of the homography cv::Mat_<float> H(3, 3, 0.0); H(0,0) = P(0,0); H(1,0) = P(1,0); H(2,0) = P(2,0); H(0,1) = P(0,1); H(1,1) = P(1,1); H(2,1) = P(2,1); H(0,2) = P(0,3); H(1,2) = P(1,3); H(2,2) = P(2,3); //std::cout << "H: " << H << std::endl; cv::Mat_<float> H_inv = H.inv(); float padding = 10; // create foot points using the pixels of the image for (int u = 0; u < imageWidth; u+=padding) { for (int v = minImageHeight; v < imageHeight; v+=padding ) { float Xw = (H_inv(0,0)*u + H_inv(0,1)*v + H_inv(0,2))/(H_inv(2,0)*u + H_inv(2,1)*v + H_inv(2,2)); float Yw = (H_inv(1,0)*u + H_inv(1,1)*v + H_inv(1,2))/(H_inv(2,0)*u + H_inv(2,1)*v + H_inv(2,2)); // now create all_candidates at different heights for (float h = -stdHeight * factorStdHeight; h <= stdHeight * factorStdHeight; h+= stepHeight) { // std::cout << h << std::endl; float wHeight = meanHeight + h; int head_v = (int)((Xw*P(1,0) + Yw*P(1,1) + wHeight*P(1,2) + P(1,3))/(Xw*P(2,0) + Yw*P(2,1) + wHeight*P(2,2) + P(2,3))); int i_height = v - head_v; if (i_height >= minImageHeight) { int head_u = (int)((Xw*P(0,0) + Yw*P(0,1) + wHeight*P(0,2) + P(0,3))/(Xw*P(2,0) + Yw*P(2,1) + wHeight*P(2,2) + P(2,3))); BoundingBox candidate; int i_width = i_height*aspectRatio; candidate.bb.x = (int)(((u + head_u)/2.0) - (i_width/2.0)); candidate.bb.y = head_v; candidate.bb.width = i_width; candidate.bb.height = i_height; candidate.world_height = wHeight; grow_candidate(candidate, 0.2); if (candidate.bb.x >= 0 && candidate.bb.x+candidate.bb.width < imageWidth && candidate.bb.y >= 0 && candidate.bb.y+candidate.bb.height < imageHeight && candidate.bb.height >= minImageHeight) { if (candidate.bb.height > max_h) max_h = candidate.bb.height; all_candidates.push_back(candidate); } } } // exit(10); } } if (maxHeight != NULL) { *maxHeight = max_h; } return all_candidates; }