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;
}
