/**
* \fn Dessin::Dessin ()
* \brief Constructeur de la classe Dessin
* \return Rien
*/
Dessin::Dessin ()
{
lecture_coordonnees_points();
/* Ajout d'évènements non gérés par défaut par DrawingArea */
this->add_events(Gdk::BUTTON_PRESS_MASK);
this->add_events(Gdk::BUTTON1_MOTION_MASK);
this->add_events(Gdk::BUTTON2_MOTION_MASK);
this->add_events(Gdk::BUTTON3_MOTION_MASK);
this->add_events(Gdk::BUTTON_RELEASE_MASK);
this->signal_event().connect(sigc::mem_fun(*this, &Dessin::on_event_happend));
/* Initialisation des valeurs de la taille de la carte avec des valeurs par défaut */
zoom_actif = 0;
hauteur_zoom = H_MAP;
largeur_zoom = H_MAP;
decalage_x_zoom = 0;
decalage_y_zoom = 0;
matrice = Matrice(194, 196);
matrice = Matrice::Zero(194, 196);
valeur_min = 0;
valeur_max = 0;
actif = 0;
initialisation = 0;
}
int main(int argc, char *argv[])
{
/* % MINIMALPATH Recherche du chemin minimum de Haut vers le bas et de
% bas vers le haut tel que dÈcrit par Luc Vincent 1998
% [sR,sC,S] = MinimalPath(I,factx)
%
% I : Image d'entrÔøΩe dans laquelle on doit trouver le
% chemin minimal
% factx : Poids de linearite [1 10]
%
% Programme par : Ramnada Chav
% ModifiÈ le 16 novembre 2007 */
typedef itk::ImageDuplicator< ImageType > DuplicatorType3D;
typedef itk::InvertIntensityImageFilter <ImageType> InvertIntensityImageFilterType;
typedef itk::StatisticsImageFilter<ImageType> StatisticsImageFilterType;
ImageType::Pointer inverted_image = image;
// if (invert)
// {
// StatisticsImageFilterType::Pointer statisticsImageFilterInput = StatisticsImageFilterType::New();
// statisticsImageFilterInput->SetInput(image);
// statisticsImageFilterInput->Update();
// double maxIm = statisticsImageFilterInput->GetMaximum();
// InvertIntensityImageFilterType::Pointer invertIntensityFilter = InvertIntensityImageFilterType::New();
// invertIntensityFilter->SetInput(image);
// invertIntensityFilter->SetMaximum(maxIm);
// invertIntensityFilter->Update();
// inverted_image = invertIntensityFilter->GetOutput();
// }
ImageType::SizeType sizeImage = image->GetLargestPossibleRegion().GetSize();
int m = sizeImage[0]; // x to change because we are in AIL
int n = sizeImage[2]; // y
int p = sizeImage[1]; // z
// create image with high values J1
DuplicatorType3D::Pointer duplicator = DuplicatorType3D::New();
duplicator->SetInputImage(inverted_image);
duplicator->Update();
ImageType::Pointer J1 = duplicator->GetOutput();
typedef itk::ImageRegionIterator<ImageType> ImageIterator3D;
ImageIterator3D vItJ1( J1, J1->GetBufferedRegion() );
vItJ1.GoToBegin();
while ( !vItJ1.IsAtEnd() )
{
vItJ1.Set(100000000);
++vItJ1;
}
// create image with high values J2
DuplicatorType3D::Pointer duplicatorJ2 = DuplicatorType3D::New();
duplicatorJ2->SetInputImage(inverted_image);
duplicatorJ2->Update();
ImageType::Pointer J2 = duplicatorJ2->GetOutput();
ImageIterator3D vItJ2( J2, J2->GetBufferedRegion() );
vItJ2.GoToBegin();
while ( !vItJ2.IsAtEnd() )
{
vItJ2.Set(100000000);
++vItJ2;
}
DuplicatorType3D::Pointer duplicatorCPixel = DuplicatorType3D::New();
duplicatorCPixel->SetInputImage(inverted_image);
duplicatorCPixel->Update();
ImageType::Pointer cPixel = duplicatorCPixel->GetOutput();
ImageType::IndexType index;
// iterate on slice from slice 1 (start=0) to slice p-2. Basically, we avoid first and last slices.
// IMPORTANT: first slice of J1 and last slice of J2 must be set to 0...
for (int x=0; x<m; x++)
{
for (int y=0; y<n; y++)
{
index[0] = x; index[1] = 0; index[2] = y;
J1->SetPixel(index, 0.0);
}
}
for (int slice=1; slice<p; slice++)
{
// 1. extract pJ = the (slice-1)th slice of the image J1
Matrice pJ = Matrice(m,n);
for (int x=0; x<m; x++)
{
for (int y=0; y<n; y++)
{
index[0] = x; index[1] = slice-1; index[2] = y;
pJ(x,y) = J1->GetPixel(index);
}
}
// 2. extract cP = the (slice)th slice of the image cPixel
Matrice cP = Matrice(m,n);
for (int x=0; x<m; x++)
{
for (int y=0; y<n; y++)
{
index[0] = x; index[1] = slice; index[2] = y;
cP(x,y) = cPixel->GetPixel(index);
}
}
// 2'
Matrice cPm = Matrice(m,n);
if (homoInt)
{
for (int x=0; x<m; x++)
{
for (int y=0; y<n; y++)
{
index[0] = x; index[1] = slice-1; index[2] = y;
cP(x,y) = cPixel->GetPixel(index);
}
}
}
// 3. Create a matrix VI with 5 slices, that are exactly a repetition of cP without borders
// multiply all elements of all slices of VI except the middle one by factx
Matrice VI[5];
for (int i=0; i<5; i++)
{
// Create VI
Matrice cP_in = Matrice(m-1, n-1);
for (int x=0; x<m-2; x++)
{
for (int y=0; y<n-2; y++)
{
cP_in(x,y) = cP(x+1,y+1);
if (i!=2)
cP_in(x,y) *= factx;
}
}
VI[i] = cP_in;
}
// 3'.
Matrice VIm[5];
if (homoInt)
{
for (int i=0; i<5; i++)
{
// Create VIm
Matrice cPm_in = Matrice(m-1, n-1);
for (int x=0; x<m-2; x++)
{
for (int y=0; y<n-2; y++)
{
cPm_in(x,y) = cPm(x+1,y+1);
if (i!=2)
cPm_in(x,y) *= factx;
}
}
VIm[i] = cPm_in;
}
}
// 4. create a matrix of 5 slices, containing pJ(vectx-1,vecty),pJ(vectx,vecty-1),pJ(vectx,vecty),pJ(vectx,vecty+1),pJ(vectx+1,vecty) where vectx=2:m-1; and vecty=2:n-1;
Matrice Jq[5];
int s = 0;
Matrice pJ_temp = Matrice(m-1, n-1);
for (int x=0; x<m-2; x++)
{
for (int y=0; y<n-2; y++)
{
pJ_temp(x,y) = pJ(x+1,y+1);
}
}
Jq[2] = pJ_temp;
for (int k=-1; k<=1; k+=2)
{
for (int l=-1; l<=1; l+=2)
{
Matrice pJ_temp = Matrice(m-1, n-1);
for (int x=0; x<m-2; x++)
{
for (int y=0; y<n-2; y++)
{
pJ_temp(x,y) = pJ(x+k+1,y+l+1);
}
}
Jq[s] = pJ_temp;
s++;
if (s==2) s++; // we deal with middle slice before
}
}
// 4'. An alternative is to minimize the difference in intensity between slices.
if (homoInt)
{
Matrice VI_temp[5];
// compute the difference between VI and VIm
for (int i=0; i<5; i++)
VI_temp[i] = VI[i] - VIm[i];
// compute the minimum value for each element of the matrices
for (int i=0; i<5; i++)
{
for (int x=0; x<m-2; x++)
{
for (int y=0; y<n-2; y++)
{
if (VI_temp[i](x,y) > 0)
VI[i](x,y) = abs(VI_temp[i](x,y));///VIm[i](x,y);
else
VI[i](x,y) = abs(VI_temp[i](x,y));///VI[i](x,y);
}
}
}
}
// 5. sum Jq and Vi voxel by voxel to produce JV
Matrice JV[5];
for (int i=0; i<5; i++)
JV[i] = VI[i] + Jq[i];
// 6. replace each pixel of the (slice)th slice of J1 with the minimum value of the corresponding column in JV
for (int x=0; x<m-2; x++)
{
for (int y=0; y<n-2; y++)
{
double min_value = 1000000;
for (int i=0; i<5; i++)
{
if (JV[i](x,y) < min_value)
min_value = JV[i](x,y);
}
index[0] = x+1; index[1] = slice; index[2] = y+1;
J1->SetPixel(index, min_value);
}
}
}
// iterate on slice from slice n-1 to slice 1. Basically, we avoid first and last slices.
// IMPORTANT: first slice of J1 and last slice of J2 must be set to 0...
for (int x=0; x<m; x++)
{
for (int y=0; y<n; y++)
{
index[0] = x; index[1] = p-1; index[2] = y;
J2->SetPixel(index, 0.0);
}
}
for (int slice=p-2; slice>=0; slice--)
{
// 1. extract pJ = the (slice-1)th slice of the image J1
Matrice pJ = Matrice(m,n);
for (int x=0; x<m; x++)
{
for (int y=0; y<n; y++)
{
index[0] = x; index[1] = slice+1; index[2] = y;
pJ(x,y) = J2->GetPixel(index);
}
}
// 2. extract cP = the (slice)th slice of the image cPixel
Matrice cP = Matrice(m,n);
for (int x=0; x<m; x++)
{
for (int y=0; y<n; y++)
{
index[0] = x; index[1] = slice; index[2] = y;
cP(x,y) = cPixel->GetPixel(index);
}
}
// 2'
Matrice cPm = Matrice(m,n);
if (homoInt)
{
for (int x=0; x<m; x++)
{
for (int y=0; y<n; y++)
{
index[0] = x; index[1] = slice+1; index[2] = y;
cPm(x,y) = cPixel->GetPixel(index);
}
}
}
// 3. Create a matrix VI with 5 slices, that are exactly a repetition of cP without borders
// multiply all elements of all slices of VI except the middle one by factx
Matrice VI[5];
for (int i=0; i<5; i++)
{
// Create VI
Matrice cP_in = Matrice(m-1, n-1);
for (int x=0; x<m-2; x++)
{
for (int y=0; y<n-2; y++)
{
cP_in(x,y) = cP(x+1,y+1);
if (i!=2)
cP_in(x,y) *= factx;
}
}
VI[i] = cP_in;
}
// 3'.
Matrice VIm[5];
if (homoInt)
{
for (int i=0; i<5; i++)
{
// Create VI
Matrice cPm_in = Matrice(m-1, n-1);
for (int x=0; x<m-2; x++)
{
for (int y=0; y<n-2; y++)
{
cPm_in(x,y) = cPm(x+1,y+1);
if (i!=2)
cPm_in(x,y) *= factx;
}
}
VIm[i] = cPm_in;
}
}
// 4. create a matrix of 5 slices, containing pJ(vectx-1,vecty),pJ(vectx,vecty-1),pJ(vectx,vecty),pJ(vectx,vecty+1),pJ(vectx+1,vecty) where vectx=2:m-1; and vecty=2:n-1;
Matrice Jq[5];
int s = 0;
Matrice pJ_temp = Matrice(m-1, n-1);
for (int x=0; x<m-2; x++)
{
for (int y=0; y<n-2; y++)
{
pJ_temp(x,y) = pJ(x+1,y+1);
}
}
Jq[2] = pJ_temp;
for (int k=-1; k<=1; k+=2)
{
for (int l=-1; l<=1; l+=2)
{
Matrice pJ_temp = Matrice(m-1, n-1);
for (int x=0; x<m-2; x++)
{
for (int y=0; y<n-2; y++)
{
pJ_temp(x,y) = pJ(x+k+1,y+l+1);
}
}
Jq[s] = pJ_temp;
s++;
if (s==2) s++; // we deal with middle slice before
}
}
// 4'. An alternative is to minimize the difference in intensity between slices.
if (homoInt)
{
Matrice VI_temp[5];
// compute the difference between VI and VIm
for (int i=0; i<5; i++)
VI_temp[i] = VI[i] - VIm[i];
// compute the minimum value for each element of the matrices
for (int i=0; i<5; i++)
{
for (int x=0; x<m-2; x++)
{
for (int y=0; y<n-2; y++)
{
if (VI_temp[i](x,y) > 0)
VI[i](x,y) = abs(VI_temp[i](x,y));///VIm[i](x,y);
else
VI[i](x,y) = abs(VI_temp[i](x,y));///VI[i](x,y);
}
}
}
}
// 5. sum Jq and Vi voxel by voxel to produce JV
Matrice JV[5];
for (int i=0; i<5; i++)
JV[i] = VI[i] + Jq[i];
// 6. replace each pixel of the (slice)th slice of J1 with the minimum value of the corresponding column in JV
for (int x=0; x<m-2; x++)
{
for (int y=0; y<n-2; y++)
{
double min_value = 10000000;
for (int i=0; i<5; i++)
{
if (JV[i](x,y) < min_value)
min_value = JV[i](x,y);
}
index[0] = x+1; index[1] = slice; index[2] = y+1;
J2->SetPixel(index, min_value);
}
}
}
// add J1 and J2 to produce "S" which is actually J1 here.
ImageIterator3D vItS( J1, J1->GetBufferedRegion() );
ImageIterator3D vItJ2b( J2, J2->GetBufferedRegion() );
vItS.GoToBegin();
vItJ2b.GoToBegin();
while ( !vItS.IsAtEnd() )
{
vItS.Set(vItS.Get()+vItJ2b.Get());
++vItS;
++vItJ2b;
}
// Find the minimal value of S for each slice and create a binary image with all the coordinates
// TO DO: the minimal path shouldn't be a pixelar path. It should be a continuous spline that is minimum.
double val_temp;
vector<CVector3> list_index;
for (int slice=1; slice<p-1; slice++)
{
double min_value_S = 10000000;
ImageType::IndexType index_min;
for (int x=1; x<m-1; x++)
{
for (int y=1; y<n-1; y++)
{
index[0] = x; index[1] = slice; index[2] = y;
val_temp = J1->GetPixel(index);
if (val_temp < min_value_S)
{
min_value_S = val_temp;
index_min = index;
}
}
}
list_index.push_back(CVector3(index_min[0], index_min[1], index_min[2]));
}
//BSplineApproximation centerline_approximator = BSplineApproximation(&list_index);
//list_index = centerline_approximator.EvaluateBSplinePoints(list_index.size());
/*// create image with high values J1
ImageType::Pointer result_bin = J2;
ImageIterator3D vItresult( result_bin, result_bin->GetBufferedRegion() );
vItresult.GoToBegin();
while ( !vItresult.IsAtEnd() )
{
vItresult.Set(0.0);
++vItresult;
}
for (int i=0; i<list_index.size(); i++)
{
index[0] = list_index[i][0]; index[1] = list_index[i][1]; index[2] = list_index[i][2];
result_bin->SetPixel(index,1.0);
}
typedef itk::ImageFileWriter< ImageType > WriterTypeM;
WriterTypeM::Pointer writerMin = WriterTypeM::New();
itk::NiftiImageIO::Pointer ioV = itk::NiftiImageIO::New();
writerMin->SetImageIO(ioV);
writerMin->SetInput( J1 ); // result_bin
writerMin->SetFileName("minimalPath.nii.gz");
try {
writerMin->Update();
}
catch( itk::ExceptionObject & e )
{
cout << "Exception thrown ! " << endl;
cout << "An error ocurred during Writing Min" << endl;
cout << "Location = " << e.GetLocation() << endl;
cout << "Description = " << e.GetDescription() << endl;
}*/
centerline = list_index;
return J1; // return image with minimal path
}
void pose_estimation_from_line_correspondence(Eigen::MatrixXf start_points,
Eigen::MatrixXf end_points,
Eigen::MatrixXf directions,
Eigen::MatrixXf points,
Eigen::MatrixXf &rot_cw,
Eigen::VectorXf &pos_cw)
{
int n = start_points.cols();
if(n != directions.cols())
{
return;
}
if(n<4)
{
return;
}
float condition_err_threshold = 1e-3;
Eigen::VectorXf cosAngleThreshold(3);
cosAngleThreshold << 1.1, 0.9659, 0.8660;
Eigen::MatrixXf optimumrot_cw(3,3);
Eigen::VectorXf optimumpos_cw(3);
std::vector<float> lineLenVec(n,1);
vfloat3 l1;
vfloat3 l2;
vfloat3 nc1;
vfloat3 Vw1;
vfloat3 Xm;
vfloat3 Ym;
vfloat3 Zm;
Eigen::MatrixXf Rot(3,3);
std::vector<vfloat3> nc_bar(n,vfloat3(0,0,0));
std::vector<vfloat3> Vw_bar(n,vfloat3(0,0,0));
std::vector<vfloat3> n_c(n,vfloat3(0,0,0));
Eigen::MatrixXf Rx(3,3);
int line_id;
for(int HowToChooseFixedTwoLines = 1 ; HowToChooseFixedTwoLines <=3 ; HowToChooseFixedTwoLines++)
{
if(HowToChooseFixedTwoLines==1)
{
#pragma omp parallel for
for(int i = 0; i < n ; i++ )
{
// to correct
float lineLen = 10;
lineLenVec[i] = lineLen;
}
std::vector<float>::iterator result;
result = std::max_element(lineLenVec.begin(), lineLenVec.end());
line_id = std::distance(lineLenVec.begin(), result);
vfloat3 temp;
temp = start_points.col(0);
start_points.col(0) = start_points.col(line_id);
start_points.col(line_id) = temp;
temp = end_points.col(0);
end_points.col(0) = end_points.col(line_id);
end_points.col(line_id) = temp;
temp = directions.col(line_id);
directions.col(0) = directions.col(line_id);
directions.col(line_id) = temp;
temp = points.col(0);
points.col(0) = points.col(line_id);
points.col(line_id) = temp;
lineLenVec[line_id] = lineLenVec[1];
lineLenVec[1] = 0;
l1 = start_points.col(0) - end_points.col(0);
l1 = l1/l1.norm();
}
for(int i = 1; i < n; i++)
{
std::vector<float>::iterator result;
result = std::max_element(lineLenVec.begin(), lineLenVec.end());
line_id = std::distance(lineLenVec.begin(), result);
l2 = start_points.col(line_id) - end_points.col(line_id);
l2 = l2/l2.norm();
lineLenVec[line_id] = 0;
MatrixXf cosAngle(3,3);
cosAngle = (l1.transpose()*l2).cwiseAbs();
if(cosAngle.maxCoeff() < cosAngleThreshold[HowToChooseFixedTwoLines])
{
break;
}
}
vfloat3 temp;
temp = start_points.col(1);
start_points.col(1) = start_points.col(line_id);
start_points.col(line_id) = temp;
temp = end_points.col(1);
end_points.col(1) = end_points.col(line_id);
end_points.col(line_id) = temp;
temp = directions.col(1);
directions.col(1) = directions.col(line_id);
directions.col(line_id) = temp;
temp = points.col(1);
points.col(1) = points.col(line_id);
points.col(line_id) = temp;
lineLenVec[line_id] = lineLenVec[1];
lineLenVec[1] = 0;
// The rotation matrix R_wc is decomposed in way which is slightly different from the description in the paper,
// but the framework is the same.
// R_wc = (Rot') * R * Rot = (Rot') * (Ry(theta) * Rz(phi) * Rx(psi)) * Rot
nc1 = x_cross(start_points.col(1),end_points.col(1));
nc1 = nc1/nc1.norm();
Vw1 = directions.col(1);
Vw1 = Vw1/Vw1.norm();
//the X axis of Model frame
Xm = x_cross(nc1,Vw1);
Xm = Xm/Xm.norm();
//the Y axis of Model frame
Ym = nc1;
//the Z axis of Model frame
Zm = x_cross(Xm,Zm);
Zm = Zm/Zm.norm();
//Rot * [Xm, Ym, Zm] = I.
Rot.col(0) = Xm;
Rot.col(1) = Ym;
Rot.col(2) = Zm;
Rot = Rot.transpose();
//rotate all the vector by Rot.
//nc_bar(:,i) = Rot * nc(:,i)
//Vw_bar(:,i) = Rot * Vw(:,i)
#pragma omp parallel for
for(int i = 0 ; i < n ; i++)
{
vfloat3 nc = x_cross(start_points.col(1),end_points.col(1));
nc = nc/nc.norm();
n_c[i] = nc;
nc_bar[i] = Rot * nc;
Vw_bar[i] = Rot * directions.col(i);
}
//Determine the angle psi, it is the angle between z axis and Vw_bar(:,1).
//The rotation matrix Rx(psi) rotates Vw_bar(:,1) to z axis
float cospsi = (Vw_bar[1])[2];//the angle between z axis and Vw_bar(:,1); cospsi=[0,0,1] * Vw_bar(:,1);.
float sinpsi= sqrt(1 - cospsi*cospsi);
Rx.row(0) = vfloat3(1,0,0);
Rx.row(1) = vfloat3(0,cospsi,-sinpsi);
Rx.row(2) = vfloat3(0,sinpsi,cospsi);
vfloat3 Zaxis = Rx * Vw_bar[1];
if(1-fabs(Zaxis[3]) > 1e-5)
{
Rx = Rx.transpose();
}
//estimate the rotation angle phi by least square residual.
//i.e the rotation matrix Rz(phi)
vfloat3 Vm2 = Rx * Vw_bar[1];
float A2 = Vm2[0];
float B2 = Vm2[1];
float C2 = Vm2[2];
float x2 = (nc_bar[1])[0];
float y2 = (nc_bar[1])[1];
float z2 = (nc_bar[1])[2];
Eigen::PolynomialSolver<double, Eigen::Dynamic> solver;
Eigen::VectorXf coeff(9);
std::vector<float> coef(9,0); //coefficients of equation (7)
Eigen::VectorXf polyDF = VectorXf::Zero(16); //%dF = ployDF(1) * t^15 + ployDF(2) * t^14 + ... + ployDF(15) * t + ployDF(16);
//construct the polynomial F'
#pragma omp parallel for
for(int i = 3 ; i < n ; i++)
{
vfloat3 Vm3 = Rx*Vw_bar[i];
float A3 = Vm3[0];
float B3 = Vm3[1];
float C3 = Vm3[2];
float x3 = (nc_bar[i])[0];
float y3 = (nc_bar[i])[1];
float z3 = (nc_bar[i])[2];
float u11 = -z2*A2*y3*B3 + y2*B2*z3*A3;
float u12 = -y2*A2*z3*B3 + z2*B2*y3*A3;
float u13 = -y2*B2*z3*B3 + z2*B2*y3*B3 + y2*A2*z3*A3 - z2*A2*y3*A3;
float u14 = -y2*B2*x3*C3 + x2*C2*y3*B3;
float u15 = x2*C2*y3*A3 - y2*A2*x3*C3;
float u21 = -x2*A2*y3*B3 + y2*B2*x3*A3;
float u22 = -y2*A2*x3*B3 + x2*B2*y3*A3;
float u23 = x2*B2*y3*B3 - y2*B2*x3*B3 - x2*A2*y3*A3 + y2*A2*x3*A3;
float u24 = y2*B2*z3*C3 - z2*C2*y3*B3;
float u25 = y2*A2*z3*C3 - z2*C2*y3*A3;
float u31 = -x2*A2*z3*A3 + z2*A2*x3*A3;
float u32 = -x2*B2*z3*B3 + z2*B2*x3*B3;
float u33 = x2*A2*z3*B3 - z2*A2*x3*B3 + x2*B2*z3*A3 - z2*B2*x3*A3;
float u34 = z2*A2*z3*C3 + x2*A2*x3*C3 - z2*C2*z3*A3 - x2*C2*x3*A3;
float u35 = -z2*B2*z3*C3 - x2*B2*x3*C3 + z2*C2*z3*B3 + x2*C2*x3*B3;
float u36 = -x2*C2*z3*C3 + z2*C2*x3*C3;
float a4 = u11*u11 + u12*u12 - u13*u13 - 2*u11*u12 + u21*u21 + u22*u22 - u23*u23
-2*u21*u22 - u31*u31 - u32*u32 + u33*u33 + 2*u31*u32;
float a3 =2*(u11*u14 - u13*u15 - u12*u14 + u21*u24 - u23*u25
- u22*u24 - u31*u34 + u33*u35 + u32*u34);
float a2 =-2*u12*u12 + u13*u13 + u14*u14 - u15*u15 + 2*u11*u12 - 2*u22*u22 + u23*u23
+ u24*u24 - u25*u25 +2*u21*u22+ 2*u32*u32 - u33*u33
- u34*u34 + u35*u35 -2*u31*u32- 2*u31*u36 + 2*u32*u36;
float a1 =2*(u12*u14 + u13*u15 + u22*u24 + u23*u25 - u32*u34 - u33*u35 - u34*u36);
float a0 = u12*u12 + u15*u15+ u22*u22 + u25*u25 - u32*u32 - u35*u35 - u36*u36 - 2*u32*u36;
float b3 =2*(u11*u13 - u12*u13 + u21*u23 - u22*u23 - u31*u33 + u32*u33);
float b2 =2*(u11*u15 - u12*u15 + u13*u14 + u21*u25 - u22*u25 + u23*u24 - u31*u35 + u32*u35 - u33*u34);
float b1 =2*(u12*u13 + u14*u15 + u22*u23 + u24*u25 - u32*u33 - u34*u35 - u33*u36);
float b0 =2*(u12*u15 + u22*u25 - u32*u35 - u35*u36);
float d0 = a0*a0 - b0*b0;
float d1 = 2*(a0*a1 - b0*b1);
float d2 = a1*a1 + 2*a0*a2 + b0*b0 - b1*b1 - 2*b0*b2;
float d3 = 2*(a0*a3 + a1*a2 + b0*b1 - b1*b2 - b0*b3);
float d4 = a2*a2 + 2*a0*a4 + 2*a1*a3 + b1*b1 + 2*b0*b2 - b2*b2 - 2*b1*b3;
float d5 = 2*(a1*a4 + a2*a3 + b1*b2 + b0*b3 - b2*b3);
float d6 = a3*a3 + 2*a2*a4 + b2*b2 - b3*b3 + 2*b1*b3;
float d7 = 2*(a3*a4 + b2*b3);
float d8 = a4*a4 + b3*b3;
std::vector<float> v { a4, a3, a2, a1, a0, b3, b2, b1, b0 };
Eigen::VectorXf vp;
vp << a4, a3, a2, a1, a0, b3, b2, b1, b0 ;
//coef = coef + v;
coeff = coeff + vp;
polyDF[0] = polyDF[0] + 8*d8*d8;
polyDF[1] = polyDF[1] + 15* d7*d8;
polyDF[2] = polyDF[2] + 14* d6*d8 + 7*d7*d7;
polyDF[3] = polyDF[3] + 13*(d5*d8 + d6*d7);
polyDF[4] = polyDF[4] + 12*(d4*d8 + d5*d7) + 6*d6*d6;
polyDF[5] = polyDF[5] + 11*(d3*d8 + d4*d7 + d5*d6);
polyDF[6] = polyDF[6] + 10*(d2*d8 + d3*d7 + d4*d6) + 5*d5*d5;
polyDF[7] = polyDF[7] + 9 *(d1*d8 + d2*d7 + d3*d6 + d4*d5);
polyDF[8] = polyDF[8] + 8 *(d1*d7 + d2*d6 + d3*d5) + 4*d4*d4 + 8*d0*d8;
polyDF[9] = polyDF[9] + 7 *(d1*d6 + d2*d5 + d3*d4) + 7*d0*d7;
polyDF[10] = polyDF[10] + 6 *(d1*d5 + d2*d4) + 3*d3*d3 + 6*d0*d6;
polyDF[11] = polyDF[11] + 5 *(d1*d4 + d2*d3)+ 5*d0*d5;
polyDF[12] = polyDF[12] + 4 * d1*d3 + 2*d2*d2 + 4*d0*d4;
polyDF[13] = polyDF[13] + 3 * d1*d2 + 3*d0*d3;
polyDF[14] = polyDF[14] + d1*d1 + 2*d0*d2;
polyDF[15] = polyDF[15] + d0*d1;
}
Eigen::VectorXd coefficientPoly = VectorXd::Zero(16);
for(int j =0; j < 16 ; j++)
{
coefficientPoly[j] = polyDF[15-j];
}
//solve polyDF
solver.compute(coefficientPoly);
const Eigen::PolynomialSolver<double, Eigen::Dynamic>::RootsType & r = solver.roots();
Eigen::VectorXd rs(r.rows());
Eigen::VectorXd is(r.rows());
std::vector<float> min_roots;
for(int j =0;j<r.rows();j++)
{
rs[j] = fabs(r[j].real());
is[j] = fabs(r[j].imag());
}
float maxreal = rs.maxCoeff();
for(int j = 0 ; j < rs.rows() ; j++ )
{
if(is[j]/maxreal <= 0.001)
{
min_roots.push_back(rs[j]);
}
}
std::vector<float> temp_v(15);
std::vector<float> poly(15);
for(int j = 0 ; j < 15 ; j++)
{
temp_v[j] = j+1;
}
for(int j = 0 ; j < 15 ; j++)
{
poly[j] = polyDF[j]*temp_v[j];
}
Eigen::Matrix<double,16,1> polynomial;
Eigen::VectorXd evaluation(min_roots.size());
for( int j = 0; j < min_roots.size(); j++ )
{
evaluation[j] = poly_eval( polynomial, min_roots[j] );
}
std::vector<float> minRoots;
for( int j = 0; j < min_roots.size(); j++ )
{
if(!evaluation[j]<=0)
{
minRoots.push_back(min_roots[j]);
}
}
if(minRoots.size()==0)
{
cout << "No solution" << endl;
return;
}
int numOfRoots = minRoots.size();
//for each minimum, we try to find a solution of the camera pose, then
//choose the one with the least reprojection residual as the optimum of the solution.
float minimalReprojectionError = 100;
// In general, there are two solutions which yields small re-projection error
// or condition error:"n_c * R_wc * V_w=0". One of the solution transforms the
// world scene behind the camera center, the other solution transforms the world
// scene in front of camera center. While only the latter one is correct.
// This can easily be checked by verifying their Z coordinates in the camera frame.
// P_c(Z) must be larger than 0 if it's in front of the camera.
for(int rootId = 0 ; rootId < numOfRoots ; rootId++)
{
float cosphi = minRoots[rootId];
float sign1 = sign_of_number(coeff[0] * pow(cosphi,4)
+ coeff[1] * pow(cosphi,3) + coeff[2] * pow(cosphi,2)
+ coeff[3] * cosphi + coeff[4]);
float sign2 = sign_of_number(coeff[5] * pow(cosphi,3)
+ coeff[6] * pow(cosphi,2) + coeff[7] * cosphi + coeff[8]);
float sinphi= -sign1*sign2*sqrt(abs(1-cosphi*cosphi));
Eigen::MatrixXf Rz(3,3);
Rz.row(0) = vfloat3(cosphi,-sinphi,0);
Rz.row(1) = vfloat3(sinphi,cosphi,0);
Rz.row(2) = vfloat3(0,0,1);
//now, according to Sec4.3, we estimate the rotation angle theta
//and the translation vector at a time.
Eigen::MatrixXf RzRxRot(3,3);
RzRxRot = Rz*Rx*Rot;
//According to the fact that n_i^C should be orthogonal to Pi^c and Vi^c, we
//have: scalarproduct(Vi^c, ni^c) = 0 and scalarproduct(Pi^c, ni^c) = 0.
//where Vi^c = Rwc * Vi^w, Pi^c = Rwc *(Pi^w - pos_cw) = Rwc * Pi^w - pos;
//Using the above two constraints to construct linear equation system Mat about
//[costheta, sintheta, tx, ty, tz, 1].
Eigen::MatrixXf Matrice(2*n-1,6);
#pragma omp parallel for
for(int i = 0 ; i < n ; i++)
{
float nxi = (nc_bar[i])[0];
float nyi = (nc_bar[i])[1];
float nzi = (nc_bar[i])[2];
Eigen::VectorXf Vm(3);
Vm = RzRxRot * directions.col(i);
float Vxi = Vm[0];
float Vyi = Vm[1];
float Vzi = Vm[2];
Eigen::VectorXf Pm(3);
Pm = RzRxRot * points.col(i);
float Pxi = Pm(1);
float Pyi = Pm(2);
float Pzi = Pm(3);
//apply the constraint scalarproduct(Vi^c, ni^c) = 0
//if i=1, then scalarproduct(Vi^c, ni^c) always be 0
if(i>1)
{
Matrice(2*i-3, 0) = nxi * Vxi + nzi * Vzi;
Matrice(2*i-3, 1) = nxi * Vzi - nzi * Vxi;
Matrice(2*i-3, 5) = nyi * Vyi;
}
//apply the constraint scalarproduct(Pi^c, ni^c) = 0
Matrice(2*i-2, 0) = nxi * Pxi + nzi * Pzi;
Matrice(2*i-2, 1) = nxi * Pzi - nzi * Pxi;
Matrice(2*i-2, 2) = -nxi;
Matrice(2*i-2, 3) = -nyi;
Matrice(2*i-2, 4) = -nzi;
Matrice(2*i-2, 5) = nyi * Pyi;
}
//solve the linear system Mat * [costheta, sintheta, tx, ty, tz, 1]' = 0 using SVD,
JacobiSVD<MatrixXf> svd(Matrice, ComputeThinU | ComputeThinV);
Eigen::MatrixXf VMat = svd.matrixV();
Eigen::VectorXf vec(2*n-1);
//the last column of Vmat;
vec = VMat.col(5);
//the condition that the last element of vec should be 1.
vec = vec/vec[5];
//the condition costheta^2+sintheta^2 = 1;
float normalizeTheta = 1/sqrt(vec[0]*vec[1]+vec[1]*vec[1]);
float costheta = vec[0]*normalizeTheta;
float sintheta = vec[1]*normalizeTheta;
Eigen::MatrixXf Ry(3,3);
Ry << costheta, 0, sintheta , 0, 1, 0 , -sintheta, 0, costheta;
//now, we get the rotation matrix rot_wc and translation pos_wc
Eigen::MatrixXf rot_wc(3,3);
rot_wc = (Rot.transpose()) * (Ry * Rz * Rx) * Rot;
Eigen::VectorXf pos_wc(3);
pos_wc = - Rot.transpose() * vec.segment(2,4);
//now normalize the camera pose by 3D alignment. We first translate the points
//on line in the world frame Pw to points in the camera frame Pc. Then we project
//Pc onto the line interpretation plane as Pc_new. So we could call the point
//alignment algorithm to normalize the camera by aligning Pc_new and Pw.
//In order to improve the accuracy of the aligment step, we choose two points for each
//lines. The first point is Pwi, the second point is the closest point on line i to camera center.
//(Pw2i = Pwi - (Pwi'*Vwi)*Vwi.)
Eigen::MatrixXf Pw2(3,n);
Pw2.setZero();
Eigen::MatrixXf Pc_new(3,n);
Pc_new.setZero();
Eigen::MatrixXf Pc2_new(3,n);
Pc2_new.setZero();
for(int i = 0 ; i < n ; i++)
{
vfloat3 nci = n_c[i];
vfloat3 Pwi = points.col(i);
vfloat3 Vwi = directions.col(i);
//first point on line i
vfloat3 Pci;
Pci = rot_wc * Pwi + pos_wc;
Pc_new.col(i) = Pci - (Pci.transpose() * nci) * nci;
//second point is the closest point on line i to camera center.
vfloat3 Pw2i;
Pw2i = Pwi - (Pwi.transpose() * Vwi) * Vwi;
Pw2.col(i) = Pw2i;
vfloat3 Pc2i;
Pc2i = rot_wc * Pw2i + pos_wc;
Pc2_new.col(i) = Pc2i - ( Pc2i.transpose() * nci ) * nci;
}
MatrixXf XXc(Pc_new.rows(), Pc_new.cols()+Pc2_new.cols());
XXc << Pc_new, Pc2_new;
MatrixXf XXw(points.rows(), points.cols()+Pw2.cols());
XXw << points, Pw2;
int nm = points.cols()+Pw2.cols();
cal_campose(XXc,XXw,nm,rot_wc,pos_wc);
pos_cw = -rot_wc.transpose() * pos_wc;
//check the condition n_c^T * rot_wc * V_w = 0;
float conditionErr = 0;
for(int i =0 ; i < n ; i++)
{
float val = ( (n_c[i]).transpose() * rot_wc * directions.col(i) );
conditionErr = conditionErr + val*val;
}
if(conditionErr/n < condition_err_threshold || HowToChooseFixedTwoLines ==3)
{
//check whether the world scene is in front of the camera.
int numLineInFrontofCamera = 0;
if(HowToChooseFixedTwoLines<3)
{
for(int i = 0 ; i < n ; i++)
{
vfloat3 P_c = rot_wc * (points.col(i) - pos_cw);
if(P_c[2]>0)
{
numLineInFrontofCamera++;
}
}
}
else
{
numLineInFrontofCamera = n;
}
if(numLineInFrontofCamera > 0.5*n)
{
//most of the lines are in front of camera, then check the reprojection error.
int reprojectionError = 0;
for(int i =0; i < n ; i++)
{
//line projection function
vfloat3 nc = rot_wc * x_cross(points.col(i) - pos_cw , directions.col(i));
float h1 = nc.transpose() * start_points.col(i);
float h2 = nc.transpose() * end_points.col(i);
float lineLen = (start_points.col(i) - end_points.col(i)).norm()/3;
reprojectionError += lineLen * (h1*h1 + h1*h2 + h2*h2) / (nc[0]*nc[0]+nc[1]*nc[1]);
}
if(reprojectionError < minimalReprojectionError)
{
optimumrot_cw = rot_wc.transpose();
optimumpos_cw = pos_cw;
minimalReprojectionError = reprojectionError;
}
}
}
}
if(optimumrot_cw.rows()>0)
{
break;
}
}
pos_cw = optimumpos_cw;
rot_cw = optimumrot_cw;
}
