ICP_API float __stdcall ICP(Point3f *verts1, Point3f *verts2, int nVerts1, int nVerts2, float *R, float *t, int maxIter)
{
PointCloud cloud1;
cloud1.pts = vector<Point3f>(verts1, verts1 + nVerts1);
cv::Mat matR(3, 3, CV_32F, R);
cv::Mat matT(1, 3, CV_32F, t);
cv::Mat verts2Mat(nVerts2, 3, CV_32F, (float*)verts2);
float error = 1;
for (int iter = 0; iter < maxIter; iter++)
{
vector<Point3f> matched1, matched2;
vector<float> distances(nVerts2);
vector<size_t> indices(nVerts2);
FindClosestPointForEach(cloud1, verts2Mat, distances, indices);
vector<float> matchDistances;
vector<int> matchIdxs(nVerts1, -1);
for (int i = 0; i < nVerts2; i++)
{
int pos = matchIdxs[indices[i]];
if (pos != -1)
{
if (matchDistances[pos] < distances[i])
continue;
}
Point3f temp;
temp.X = verts2Mat.at<float>(i, 0);
temp.Y = verts2Mat.at<float>(i, 1);
temp.Z = verts2Mat.at<float>(i, 2);
if (pos == -1)
{
matched1.push_back(verts1[indices[i]]);
matched2.push_back(temp);
matchDistances.push_back(distances[i]);
matchIdxs[indices[i]] = matched1.size() - 1;
}
else
{
matched2[pos] = temp;
matchDistances[pos] = distances[i];
}
}
RejectOutlierMatches(matched1, matched2, matchDistances, 2.5);
//error = 0;
//for (int i = 0; i < matchDistances.size(); i++)
//{
// error += sqrt(matchDistances[i]);
//}
//error /= matchDistances.size();
//cout << error << endl;
cv::Mat matched1MatCv(matched1.size(), 3, CV_32F, matched1.data());
cv::Mat matched2MatCv(matched2.size(), 3, CV_32F, matched2.data());
cv::Mat tempT;
cv::reduce(matched1MatCv - matched2MatCv, tempT, 0, CV_REDUCE_AVG);
for (int i = 0; i < verts2Mat.rows; i++)
{
verts2Mat.row(i) += tempT;
}
for (int i = 0; i < matched2MatCv.rows; i++)
{
matched2MatCv.row(i) += tempT;
}
cv::Mat M = matched2MatCv.t() * matched1MatCv;
cv::SVD svd;
svd(M);
cv::Mat tempR = svd.u * svd.vt;
double det = cv::determinant(tempR);
if (det < 0)
{
cv::Mat temp = cv::Mat::eye(3, 3, CV_32F);
temp.at<float>(2, 2) = -1;
tempR = svd.u * temp * svd.vt;
}
verts2Mat = verts2Mat * tempR;
matT += tempT * matR.t();
matR = matR * tempR;
}
memcpy(verts2, verts2Mat.data, verts2Mat.rows * sizeof(float) * 3);
memcpy(R, matR.data, 9 * sizeof(float));
memcpy(t, matT.data, 3 * sizeof(float));
return error;
}
bool VersionManager::isResNeedUpdate()
{
return svd().ver.is_high_to(rvd().ver);
}
void ForecastMachine::smap_prediction(const size_t start, const size_t end)
{
size_t curr_pred, effective_nn, E = data_vectors[0].size();
double avg_distance;
vec weights;
std::vector<size_t> nearest_neighbors;
MatrixXd A, S_inv;
VectorXd B, S, x;
double max_s, pred;
std::vector<size_t> temp_lib;
for(size_t k = start; k < end; ++k)
{
curr_pred = which_pred[k];
// find nearest neighbors
if(CROSS_VALIDATION)
{
temp_lib = which_lib;
adjust_lib(curr_pred);
nearest_neighbors = find_nearest_neighbors(distances.innerVector(curr_pred));
which_lib = temp_lib;
}
else
{
nearest_neighbors = find_nearest_neighbors(distances.innerVector(curr_pred));
}
effective_nn = nearest_neighbors.size();
if(effective_nn == 0)
{
predicted[curr_pred] = qnan;
LOG_WARNING("no nearest neighbors found; using NA for forecast");
continue;
}
weights.assign(effective_nn, 1.0); // default is for theta = 0
if(theta > 0.0)
{
// compute average distance
avg_distance = 0;
for(auto& neighbor: nearest_neighbors)
{
avg_distance += distances.coeff(curr_pred, neighbor);
}
avg_distance /= effective_nn;
// compute weights
for(size_t i = 0; i < effective_nn; ++i)
weights[i] = exp(-theta * distances.coeff(curr_pred, nearest_neighbors[i]) / avg_distance);
}
// setup matrices for SVD
A.resize(effective_nn, E+1);
B.resize(effective_nn);
for(size_t i = 0; i < effective_nn; ++i)
{
B(i) = weights[i] * targets[nearest_neighbors[i]];
for(size_t j = 0; j < E; ++j)
A(i, j) = weights[i] * data_vectors[nearest_neighbors[i]][j];
A(i, E) = weights[i];
}
// perform SVD
JacobiSVD<MatrixXd> svd(A, ComputeThinU | ComputeThinV);
// remove singular values close to 0
S = svd.singularValues();
S_inv = MatrixXd::Zero(E+1, E+1);
max_s = S(0) * 1e-5;
for(size_t j = 0; j <= E; ++j)
{
if(S(j) >= max_s)
S_inv(j, j) = 1/S(j);
}
// perform back-substitution to solve
x = svd.matrixV() * S_inv * svd.matrixU().transpose() * B;
pred = 0;
for(size_t j = 0; j < E; ++j)
pred += x(j) * data_vectors[curr_pred][j];
pred += x(E);
if(SAVE_SMAP_COEFFICIENTS)
{
for(size_t j = 0; j <= E; ++j)
smap_coefficients[curr_pred][j] = x(j);
}
// save prediction
predicted[curr_pred] = pred;
}
return;
}
void Foam::FitData<FitDataType, ExtendedStencil, Polynomial>::calcFit
(
scalarList& coeffsi,
const List<point>& C,
const scalar wLin,
const label facei
)
{
vector idir(1,0,0);
vector jdir(0,1,0);
vector kdir(0,0,1);
findFaceDirs(idir, jdir, kdir, facei);
// Setup the point weights
scalarList wts(C.size(), scalar(1));
wts[0] = centralWeight_;
if (linearCorrection_)
{
wts[1] = centralWeight_;
}
// Reference point
point p0 = this->mesh().faceCentres()[facei];
// Info << "Face " << facei << " at " << p0 << " stencil points at:\n"
// << C - p0 << endl;
// p0 -> p vector in the face-local coordinate system
vector d;
// Local coordinate scaling
scalar scale = 1;
// Matrix of the polynomial components
scalarRectangularMatrix B(C.size(), minSize_, scalar(0));
for(label ip = 0; ip < C.size(); ip++)
{
const point& p = C[ip];
d.x() = (p - p0)&idir;
d.y() = (p - p0)&jdir;
# ifndef SPHERICAL_GEOMETRY
d.z() = (p - p0)&kdir;
# else
d.z() = mag(p) - mag(p0);
# endif
if (ip == 0)
{
scale = cmptMax(cmptMag((d)));
}
// Scale the radius vector
d /= scale;
Polynomial::addCoeffs
(
B[ip],
d,
wts[ip],
dim_
);
}
// Additional weighting for constant and linear terms
for(label i = 0; i < B.n(); i++)
{
B[i][0] *= wts[0];
B[i][1] *= wts[0];
}
// Set the fit
label stencilSize = C.size();
coeffsi.setSize(stencilSize);
bool goodFit = false;
for(int iIt = 0; iIt < 8 && !goodFit; iIt++)
{
SVD svd(B, SMALL);
scalar maxCoeff = 0;
label maxCoeffi = 0;
for(label i=0; i<stencilSize; i++)
{
coeffsi[i] = wts[0]*wts[i]*svd.VSinvUt()[0][i];
if (mag(coeffsi[i]) > maxCoeff)
{
maxCoeff = mag(coeffsi[i]);
maxCoeffi = i;
}
}
if (linearCorrection_)
{
goodFit =
(mag(coeffsi[0] - wLin) < linearLimitFactor_*wLin)
&& (mag(coeffsi[1] - (1 - wLin)) < linearLimitFactor_*(1 - wLin))
&& maxCoeffi <= 1;
}
else
{
// Upwind: weight on face is 1.
goodFit =
(mag(coeffsi[0] - 1.0) < linearLimitFactor_*1.0)
&& maxCoeffi <= 1;
}
// if (goodFit && iIt > 0)
// {
// Info << "FitData<Polynomial>::calcFit"
// << "(const List<point>& C, const label facei" << nl
// << "Can now fit face " << facei << " iteration " << iIt
// << " with sum of weights " << sum(coeffsi) << nl
// << " Weights " << coeffsi << nl
// << " Linear weights " << wLin << " " << 1 - wLin << nl
// << " sing vals " << svd.S() << endl;
// }
if (!goodFit) // (not good fit so increase weight in the centre and weight
// for constant and linear terms)
{
// if (iIt == 7)
// {
// WarningIn
// (
// "FitData<Polynomial>::calcFit"
// "(const List<point>& C, const label facei"
// ) << "Cannot fit face " << facei << " iteration " << iIt
// << " with sum of weights " << sum(coeffsi) << nl
// << " Weights " << coeffsi << nl
// << " Linear weights " << wLin << " " << 1 - wLin << nl
// << " sing vals " << svd.S() << endl;
// }
wts[0] *= 10;
if (linearCorrection_)
{
wts[1] *= 10;
}
for(label j = 0; j < B.m(); j++)
{
B[0][j] *= 10;
B[1][j] *= 10;
}
for(label i = 0; i < B.n(); i++)
{
B[i][0] *= 10;
B[i][1] *= 10;
}
}
}
if (goodFit)
{
if (linearCorrection_)
{
// Remove the uncorrected linear coefficients
coeffsi[0] -= wLin;
coeffsi[1] -= 1 - wLin;
}
else
{
// Remove the uncorrected upwind coefficients
coeffsi[0] -= 1.0;
}
}
else
{
// if (debug)
// {
WarningIn
(
"FitData<Polynomial>::calcFit(..)"
) << "Could not fit face " << facei
<< " Weights = " << coeffsi
<< ", reverting to linear." << nl
<< " Linear weights " << wLin << " " << 1 - wLin << endl;
// }
coeffsi = 0;
}
}
inline
bool
op_princomp::direct_princomp
(
Mat< std::complex<typename T1::pod_type> >& coeff_out,
Mat< std::complex<typename T1::pod_type> >& score_out,
Col< typename T1::pod_type >& latent_out,
const Base< std::complex<typename T1::pod_type>, T1 >& X,
const typename arma_cx_only<typename T1::elem_type>::result* junk
)
{
arma_extra_debug_sigprint();
arma_ignore(junk);
typedef typename T1::pod_type T;
typedef std::complex<T> eT;
const unwrap_check<T1> Y( X.get_ref(), score_out );
const Mat<eT>& in = Y.M;
const uword n_rows = in.n_rows;
const uword n_cols = in.n_cols;
if(n_rows > 1) // more than one sample
{
// subtract the mean - use score_out as temporary matrix
score_out = in; score_out.each_row() -= mean(in);
// singular value decomposition
Mat<eT> U;
Col< T> s;
const bool svd_ok = svd(U, s, coeff_out, score_out);
if(svd_ok == false) { return false; }
// normalize the eigenvalues
s /= std::sqrt( double(n_rows - 1) );
// project the samples to the principals
score_out *= coeff_out;
if(n_rows <= n_cols) // number of samples is less than their dimensionality
{
score_out.cols(n_rows-1,n_cols-1).zeros();
Col<T> s_tmp = zeros< Col<T> >(n_cols);
s_tmp.rows(0,n_rows-2) = s.rows(0,n_rows-2);
s = s_tmp;
}
// compute the eigenvalues of the principal vectors
latent_out = s%s;
}
else // 0 or 1 samples
{
coeff_out.eye(n_cols, n_cols);
score_out.copy_size(in);
score_out.zeros();
latent_out.set_size(n_cols);
latent_out.zeros();
}
return true;
}
void HomographyModel::estimate_from_minimal_set(const vector< ScoredMatch> &data_points)
{ // given m points estimate the parameters vector
// based on vxl rrel_homography2d_est :: fit_from_minimal_set
if ( data_points.size() < get_num_points_to_estimate())
throw runtime_error("Not enough points to estimate the HomographyModel parameters");
vnl_matrix< double > A(9, 9, 0.0);
for ( int i=0; i < get_num_points_to_estimate(); i+=1 )
{ // for i = 0,1,2,3
vgl_homg_point_2d<double> from_point(data_points[i].feature_a->x, data_points[i].feature_a->y);
vgl_homg_point_2d<double> to_point(data_points[i].feature_b->x, data_points[i].feature_b->y);
if (false)
{ // just for debugging
printf("from_points[%i] -> to_points[%i] ==", i,i);
cout << from_point << " -> " << to_point << endl;
}
A( 2*i, 0 ) = A( 2*i+1, 3 ) = from_point.x() * to_point.w();
A( 2*i, 1 ) = A( 2*i+1, 4 ) = from_point.y() * to_point.w();
A( 2*i, 2 ) = A( 2*i+1, 5 ) = from_point.w() * to_point.w();
A( 2*i, 6 ) = -1 * from_point.x() * to_point.x();
A( 2*i, 7 ) = -1 * from_point.y() * to_point.x();
A( 2*i, 8 ) = -1 * from_point.w() * to_point.x();
A( 2*i+1, 6 ) = -1 * from_point.x() * to_point.y();
A( 2*i+1, 7 ) = -1 * from_point.y() * to_point.y();
A( 2*i+1, 8 ) = -1 * from_point.w() * to_point.y();
}
vnl_svd<double> svd( A, 1.0e-8 );
if (false)
{ // just for debugging
cout << "A == " << A << endl;
cout << "svd(A) == " << svd << endl;
}
const unsigned int homog_dof_ = 8;
if ( svd.rank() < homog_dof_ )
{
// singular fit
if (true)
{ // just for debugging
cout << "svd.rank() == " << svd.rank() << endl;
for ( unsigned int i=0; i<get_num_points_to_estimate(); ++i )
{
vgl_homg_point_2d<double> from_point(data_points[i].feature_a->x, data_points[i].feature_a->y);
vgl_homg_point_2d<double> to_point(data_points[i].feature_b->x, data_points[i].feature_b->y);
cout << "from->to point[i]-> " << from_point << "->" << to_point << endl;
}
}
throw runtime_error("HomographyModel::estimate_from_minimal_set failed");
}
vnl_vector<double> params = svd.nullvector();
parameters.resize(get_num_parameters());
if (params.size() != parameters.size() )
{
throw runtime_error("HomographyModel::estimate_from_minimal_set internal error");
}
for ( unsigned int i=0; i<get_num_parameters(); i+=1 )
{
parameters[i] = params[i]; // copy the result
}
if ( false ) {
cout << "HomographyModel parameters: " << parameters << endl;
}
return;
} // end of 'HomographyModel::estimate_from_minimal_set'
// Overloaded version: Return singular values only.
//---------------------------------------------------------
DVec& svd(const DMat& mat)
//---------------------------------------------------------
{
DMat U, VT;
return svd(mat, U, VT, 'N', 'N');
}
inline
void r_and_t(MatrixXf &rot_cw, VectorXf &pos_cw,MatrixXf start_points, MatrixXf end_points,
MatrixXf P1w,MatrixXf P2w,MatrixXf initRot_cw,VectorXf initPos_cw,
int maxIterNum,float TerminateTh,int nargin)
{
if(nargin<6)
{
return;
}
if(nargin<8)
{
maxIterNum = 8;
TerminateTh = 1e-5;
}
int n = start_points.cols();
if(n != end_points.cols() || n!= P1w.cols() || n!= P2w.cols())
{
return;
}
if(n<4)
{
return;
}
//first compute the weight of each line and the normal of
//the interpretation plane passing through to camera center and the line
VectorXf w = VectorXf::Zero(n);
MatrixXf nc = MatrixXf::Zero(3,n);
for(int i = 0 ; i < n ; i++)
{
//the weight of a line is the inverse of its image length
w[i] = 1/(start_points.col(i)-end_points.col(i)).norm();
vfloat3 v1 = start_points.col(i);
vfloat3 v2 = end_points.col(i);
vfloat3 temp = v1.cross(v2);
nc.col(i) = temp/temp.norm();
}
MatrixXf rot_wc = initPos_cw.transpose();
MatrixXf pos_wc = - initRot_cw.transpose() * initPos_cw;
for(int iter = 1 ; iter < maxIterNum ; iter++)
{
//construct the equation (31)
MatrixXf A = MatrixXf::Zero(6,7);
MatrixXf C = MatrixXf::Zero(3,3);
MatrixXf D = MatrixXf::Zero(3,3);
MatrixXf F = MatrixXf::Zero(3,3);
vfloat3 c_bar = vfloat3(0,0,0);
vfloat3 d_bar = vfloat3(0,0,0);
for(int i = 0 ; i < n ; i++)
{
//for first point on line
vfloat3 Pi = rot_wc * P1w.col(i);
vfloat3 Ni = nc.col(i);
float wi = w[i];
vfloat3 bi = Pi.cross(Ni);
C = C + wi*Ni*Ni.transpose();
D = D + wi*bi*bi.transpose();
F = F + wi*Ni*bi.transpose();
vfloat3 tempi = Pi + pos_wc;
float scale = Ni.transpose() * tempi;
scale *= wi;
c_bar = c_bar + scale * Ni;
d_bar = d_bar + scale*bi;
//for second point on line
Pi = rot_wc * P2w.col(i);
Ni = nc.col(i);
wi = w[i];
bi = Pi.cross(Ni);
C = C + wi*Ni*Ni.transpose();
D = D + wi*bi*bi.transpose();
F = F + wi*Ni*bi.transpose();
scale = (Ni.transpose() * (Pi + pos_wc));
scale *= wi;
c_bar = c_bar + scale * Ni;
d_bar = d_bar + scale * bi;
}
A.block<3,3>(0,0) = C;
A.block<3,3>(0,3) = F;
(A.col(6)).segment(0,2) = c_bar;
A.block<3,3>(3,0) = F.transpose();
A.block<3,3>(2,2) = D;
(A.col(6)).segment(3,5) = d_bar;
//sovle the system by using SVD;
JacobiSVD<MatrixXf> svd(A, ComputeThinU | ComputeThinV);
VectorXf vec(7);
//the last column of Vmat;
vec = (svd.matrixV()).col(6);
//the condition that the last element of vec should be 1.
vec = vec/vec[6];
//update the rotation and translation parameters;
vfloat3 dT = vec.segment(0,2);
vfloat3 dOmiga = vec.segment(3,5);
MatrixXf rtemp(3,3);
rtemp << 1, -dOmiga[2], dOmiga[1], dOmiga[2], 1, -dOmiga[1], -dOmiga[1], dOmiga[0], 1;
rot_wc = rtemp * rot_wc;
//newRot_wc = ( I + [dOmiga]x ) oldRot_wc
//may be we can compute new R using rodrigues(r+dr)
pos_wc = pos_wc + dT;
if(dT.norm() < TerminateTh && dOmiga.norm() < 0.1*TerminateTh)
{
break;
}
}
rot_cw = rot_wc.transpose();
pos_cw = -rot_cw * pos_wc;
}
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;
}
void Mesher::check_feature()
{
auto contour = get_contour();
const auto normals = get_normals(contour);
// Find the largest cone and the normals that enclose
// the largest angle as n0, n1.
float theta = 1;
Vec3f n0, n1;
for (auto ni : normals)
{
for (auto nj : normals)
{
float dot = ni.dot(nj);
if (dot < theta)
{
theta = dot;
n0 = ni;
n1 = nj;
}
}
}
// If there isn't a feature in this fan, then return immediately.
if (theta > 0.9)
return;
// Decide whether this is a corner or edge feature.
const Vec3f nstar = n0.cross(n1);
float phi = 0;
for (auto n : normals)
phi = fmax(phi, fabs(nstar.dot(n)));
bool edge = phi < 0.7;
// Find the center of the contour.
Vec3f center(0, 0, 0);
for (auto c : contour)
center += c;
center /= contour.size();
// Construct the matrices for use in our least-square fit.
Eigen::MatrixX3d A(normals.size(), 3);
{
int i=0;
for (auto n : normals)
A.row(i++) << n.transpose();
}
// When building the second matrix, shift position values to be centered
// about the origin (because that's what the least-squares fit will
// minimize).
Eigen::VectorXd B(normals.size(), 1);
{
auto n = normals.begin();
auto c = contour.begin();
int i=0;
while (n != normals.end())
B.row(i++) << (n++)->dot(*(c++) - center);
}
// Use singular value decomposition to solve the least-squares fit.
Eigen::JacobiSVD<Eigen::MatrixX3d> svd(A, Eigen::ComputeFullU |
Eigen::ComputeFullV);
// Set the smallest singular value to zero to make fitting happier.
if (edge)
{
auto singular = svd.singularValues();
svd.setThreshold(singular.minCoeff() / singular.maxCoeff() * 1.01);
}
// Solve for the new point's position.
const Vec3f new_pt = svd.solve(B) + center;
// Erase this triangle fan, as we'll be inserting a vertex in the center.
triangles.erase(fan_start, voxel_start);
// Construct a new triangle fan.
contour.push_back(contour.front());
{
auto p0 = contour.begin();
auto p1 = contour.begin();
p1++;
while (p1 != contour.end())
push_swappable_triangle(Triangle(*(p0++), *(p1++), new_pt));
}
}
inline
void cal_campose(Eigen::MatrixXf XXc,Eigen::MatrixXf XXw,
int n,Eigen::MatrixXf &R2,Eigen::VectorXf &t2)
{
//A
Eigen::MatrixXf X = XXw;
//B
Eigen::MatrixXf Y = XXc;
Eigen::MatrixXf eyen(n,n);
eyen = Eigen::MatrixXf::Identity(n,n);
Eigen::MatrixXf ones(n,n);
ones.setOnes();
Eigen::MatrixXf K(n,n);
K = eyen - ones/n;
vfloat3 ux;
for(int i =0; i < n; i++)
{
ux = ux + X.col(i);
}
ux = ux/n;
vfloat3 uy;
for(int i =0; i < n; i++)
{
uy = uy + Y.col(i);
}
uy = uy/n;
Eigen::MatrixXf XK(3,n);
XK = X*K;
Eigen::MatrixXf XKarre(3,n);
for(int i = 0 ; i < n ; i++)
{
XKarre(0,i) = XK(0,i)*XK(0,i);
XKarre(1,i) = XK(1,i)*XK(1,i);
XKarre(2,i) = XK(2,i)*XK(2,i);
}
Eigen::VectorXf sumXKarre(n);
float sigmx2 = 0;
for(int i = 0 ; i < n ; i++)
{
sumXKarre[i] = XKarre(0,i) + XKarre(1,i) + XKarre(2,i);
sigmx2 += sumXKarre[i];
}
sigmx2 /=n;
Eigen::MatrixXf SXY(3,3);
SXY = Y*K*(X.transpose())/n;
JacobiSVD<MatrixXf> svd(SXY, ComputeThinU | ComputeThinV);
Eigen::MatrixXf S(3,3);
S = Eigen::MatrixXf::Identity(3,3);
if(SXY.determinant() < 0)
{
S(3,3) = -1;
}
R2 = svd.matrixU() * S * (svd.matrixV()).transpose();
Eigen::MatrixXf D(3,3);
D.setZero();
for(int i = 0 ; i < svd.singularValues().size() ; i++)
{
D(i,i) = (svd.singularValues())[i];
}
float c2 = (D*S).trace()/sigmx2;
t2 = uy - c2*R2*ux;
vfloat3 Xx = R2.col(0);
vfloat3 Yy = R2.col(1);
vfloat3 Zz = R2.col(2);
if((x_cross(Xx,Yy)-Zz).norm()>2e-2)
{
R2.col(2) = -Zz;
}
}
void solveLinear(Double_t eps = 1.e-12)
{
cout << "Perform the fit y = c0 + c1 * x in four different ways" << endl;
const Int_t nrVar = 2;
const Int_t nrPnts = 4;
Double_t ax[] = {0.0,1.0,2.0,3.0};
Double_t ay[] = {1.4,1.5,3.7,4.1};
Double_t ae[] = {0.5,0.2,1.0,0.5};
// Make the vectors 'Use" the data : they are not copied, the vector data
// pointer is just set appropriately
TVectorD x; x.Use(nrPnts,ax);
TVectorD y; y.Use(nrPnts,ay);
TVectorD e; e.Use(nrPnts,ae);
TMatrixD A(nrPnts,nrVar);
TMatrixDColumn(A,0) = 1.0;
TMatrixDColumn(A,1) = x;
cout << " - 1. solve through Normal Equations" << endl;
const TVectorD c_norm = NormalEqn(A,y,e);
cout << " - 2. solve through SVD" << endl;
// numerically preferred method
// first bring the weights in place
TMatrixD Aw = A;
TVectorD yw = y;
for (Int_t irow = 0; irow < A.GetNrows(); irow++) {
TMatrixDRow(Aw,irow) *= 1/e(irow);
yw(irow) /= e(irow);
}
TDecompSVD svd(Aw);
Bool_t ok;
const TVectorD c_svd = svd.Solve(yw,ok);
cout << " - 3. solve with pseudo inverse" << endl;
const TMatrixD pseudo1 = svd.Invert();
TVectorD c_pseudo1 = yw;
c_pseudo1 *= pseudo1;
cout << " - 4. solve with pseudo inverse, calculated brute force" << endl;
TMatrixDSym AtA(TMatrixDSym::kAtA,Aw);
const TMatrixD pseudo2 = AtA.Invert() * Aw.T();
TVectorD c_pseudo2 = yw;
c_pseudo2 *= pseudo2;
cout << " - 5. Minuit through TGraph" << endl;
TGraphErrors *gr = new TGraphErrors(nrPnts,ax,ay,0,ae);
TF1 *f1 = new TF1("f1","pol1",0,5);
gr->Fit("f1","Q");
TVectorD c_graph(nrVar);
c_graph(0) = f1->GetParameter(0);
c_graph(1) = f1->GetParameter(1);
// Check that all 4 answers are identical within a certain
// tolerance . The 1e-12 is somewhat arbitrary . It turns out that
// the TGraph fit is different by a few times 1e-13.
Bool_t same = kTRUE;
same &= VerifyVectorIdentity(c_norm,c_svd,0,eps);
same &= VerifyVectorIdentity(c_norm,c_pseudo1,0,eps);
same &= VerifyVectorIdentity(c_norm,c_pseudo2,0,eps);
same &= VerifyVectorIdentity(c_norm,c_graph,0,eps);
if (same)
cout << " All solutions are the same within tolerance of " << eps << endl;
else
cout << " Some solutions differ more than the allowed tolerance of " << eps << endl;
}
Alignment::Alignment(PharmacophoreMap& fMap) :
_refMap(),
_dbMap(),
_refCenter(),
_dbCenter(),
_refRotMat(3,3,0.0),
_dbRotMat(3,3,0.0),
_dCdq(4,0.0),
_d2Cdq2(4,4,0.0),
_grad(4,0.0),
_AkA(fMap.size()),
_nbrPoints(0),
_nbrExcl(0)
{
// compute centers of the two pharmacophore sets
PharmacophoreMap::iterator mi;
unsigned int nbrMatch(0);
// compute the centroid of the pharmacophores
double V1(0.0), v1(0.0), V2(0.0), v2(0.0);
for (mi = fMap.begin(); mi != fMap.end(); ++mi)
{
if (mi->first->func == EXCL)
{
_nbrExcl++;
continue; // do not use exclusion spheres when computing the center
}
_nbrPoints++;
v1 = GCI * pow(PI/mi->first->alpha,1.5);
V1 += v1;
_refCenter.x += v1 * mi->first->point.x;
_refCenter.y += v1 * mi->first->point.y;
_refCenter.z += v1 * mi->first->point.z;
v2 = GCI * pow(PI/mi->second->alpha,1.5);
V2 += v2;
_dbCenter.x += v2 * mi->second->point.x;
_dbCenter.y += v2 * mi->second->point.y;
_dbCenter.z += v2 * mi->second->point.z;
++nbrMatch;
}
_refCenter.x /= V1;
_refCenter.y /= V1;
_refCenter.z /= V1;
_dbCenter.x /= V2;
_dbCenter.y /= V2;
_dbCenter.z /= V2;
// translate the pharmacophores to the centers
// and compute center of mass matrix
SiMath::Matrix mass1(3,3,0.0);
SiMath::Matrix mass2(3,3,0.0);
for (mi = fMap.begin(); mi != fMap.end(); ++mi)
{
PharmacophorePoint p1, p2;
p1.point.x = mi->first->point.x - _refCenter.x;
p1.point.y = mi->first->point.y - _refCenter.y;
p1.point.z = mi->first->point.z - _refCenter.z;
p1.func = mi->first->func;
p1.alpha = mi->first->alpha;
p1.normal.x = mi->first->normal.x - mi->first->point.x;
p1.normal.y = mi->first->normal.y - mi->first->point.y;
p1.normal.z = mi->first->normal.z - mi->first->point.z;
p2.point.x = mi->second->point.x - _dbCenter.x;
p2.point.y = mi->second->point.y - _dbCenter.y;
p2.point.z = mi->second->point.z - _dbCenter.z;
p2.func = mi->second->func;
p2.alpha = mi->second->alpha;
p2.normal.x = mi->second->normal.x - mi->second->point.x;
p2.normal.y = mi->second->normal.y - mi->second->point.y;
p2.normal.z = mi->second->normal.z - mi->second->point.z;
if (mi->first->func != EXCL)
{
v1 = GCI * pow(PI/mi->first->alpha,1.5);
mass1[0][0] += v1 * p1.point.x * p1.point.x;
mass1[0][1] += v1 * p1.point.x * p1.point.y;
mass1[0][2] += v1 * p1.point.x * p1.point.z;
mass1[1][1] += v1 * p1.point.y * p1.point.y;
mass1[1][2] += v1 * p1.point.y * p1.point.z;
mass1[2][2] += v1 * p1.point.z * p1.point.z;
v2 = GCI * pow(PI/mi->second->alpha,1.5);
mass2[0][0] += v2 * p2.point.x * p2.point.x;
mass2[0][1] += v2 * p2.point.x * p2.point.y;
mass2[0][2] += v2 * p2.point.x * p2.point.z;
mass2[1][1] += v2 * p2.point.y * p2.point.y;
mass2[1][2] += v2 * p2.point.y * p2.point.z;
mass2[2][2] += v2 * p2.point.z * p2.point.z;
}
// add new points to local maps
_refMap.push_back(p1);
_dbMap.push_back(p2);
}
// use SVD to compute best rotations
// set lower triangle
mass1[1][0] = mass1[0][1];
mass1[2][0] = mass1[0][2];
mass1[2][1] = mass1[1][2];
// normalize mass matrix
mass1 /= V1;
// compute SVD of the mass matrix
SiMath::SVD svd(mass1, true, true);
_refRotMat = svd.getU();
// check if determinant is 1, otherwise it is a mirroring instead of rotation
double det = _refRotMat[0][0]*_refRotMat[1][1]*_refRotMat[2][2]
+ _refRotMat[2][1]*_refRotMat[1][0]*_refRotMat[0][2]
+ _refRotMat[0][1]*_refRotMat[1][2]*_refRotMat[2][0]
- _refRotMat[0][0]*_refRotMat[1][2]*_refRotMat[2][1]
- _refRotMat[1][1]*_refRotMat[2][0]*_refRotMat[0][2]
- _refRotMat[2][2]*_refRotMat[0][1]*_refRotMat[1][0];
// check if it is a rotation matrix and not a mirroring
if (det < 0)
{
// switch sign of third column
_refRotMat[0][2] = -_refRotMat[0][2];
_refRotMat[1][2] = -_refRotMat[1][2];
_refRotMat[2][2] = -_refRotMat[2][2];
}
// set lower triangle
mass2[1][0] = mass2[0][1];
mass2[2][0] = mass2[0][2];
mass2[2][1] = mass2[1][2];
// normalize mass matrix
mass2 /= V2;
// compute SVD of the mass matrix
SiMath::SVD svd2(mass2, true, true);
_dbRotMat = svd2.getU();
// check if determinant is 1, otherwise it is a mirroring instead of rotation
det = _dbRotMat[0][0]*_dbRotMat[1][1]*_dbRotMat[2][2]
+ _dbRotMat[2][1]*_dbRotMat[1][0]*_dbRotMat[0][2]
+ _dbRotMat[0][1]*_dbRotMat[1][2]*_dbRotMat[2][0]
- _dbRotMat[0][0]*_dbRotMat[1][2]*_dbRotMat[2][1]
- _dbRotMat[1][1]*_dbRotMat[2][0]*_dbRotMat[0][2]
- _dbRotMat[2][2]*_dbRotMat[0][1]*_dbRotMat[1][0];
// checif if it is a rotation matrix and not a mirroring
if (det < 0)
{
// switch sign of third column
_dbRotMat[0][2] = -_dbRotMat[0][2];
_dbRotMat[1][2] = -_dbRotMat[1][2];
_dbRotMat[2][2] = -_dbRotMat[2][2];
}
// rotate points towards main axes
for (unsigned int i(0); i < _refMap.size(); ++i)
{
// Rotate points
double x = _refMap[i].point.x;
double y = _refMap[i].point.y;
double z = _refMap[i].point.z;
_refMap[i].point.x = _refRotMat[0][0]*x + _refRotMat[1][0]*y + _refRotMat[2][0]*z;
_refMap[i].point.y = _refRotMat[0][1]*x + _refRotMat[1][1]*y + _refRotMat[2][1]*z;
_refMap[i].point.z = _refRotMat[0][2]*x + _refRotMat[1][2]*y + _refRotMat[2][2]*z;
x = _dbMap[i].point.x;
y = _dbMap[i].point.y;
z = _dbMap[i].point.z;
_dbMap[i].point.x = _dbRotMat[0][0]*x + _dbRotMat[1][0]*y + _dbRotMat[2][0]*z;
_dbMap[i].point.y = _dbRotMat[0][1]*x + _dbRotMat[1][1]*y + _dbRotMat[2][1]*z;
_dbMap[i].point.z = _dbRotMat[0][2]*x + _dbRotMat[1][2]*y + _dbRotMat[2][2]*z;
double dx = _refMap[i].point.x - _dbMap[i].point.x;
double dx2 = dx * dx;
double dy = _refMap[i].point.y - _dbMap[i].point.y;
double dy2 = dy * dy;
double dz = _refMap[i].point.z - _dbMap[i].point.z;
double dz2 = dz * dz;
double sx = _refMap[i].point.x + _dbMap[i].point.x;
double sx2 = sx * sx;
double sy = _refMap[i].point.y + _dbMap[i].point.y;
double sy2 = sy * sy;
double sz = _refMap[i].point.z + _dbMap[i].point.z;
double sz2 = sz * sz;
_AkA[i] = new SiMath::Matrix(4,4,0.0);
(*_AkA[i])[0][0] = dx2 + dy2 + dz2;
(*_AkA[i])[0][1] = dy*sz - sy*dz;
(*_AkA[i])[0][2] = sx*dz - dx*sz;
(*_AkA[i])[0][3] = dx*sy - sx*dy;
(*_AkA[i])[1][0] = (*_AkA[i])[0][1];
(*_AkA[i])[1][1] = dx2 + sy2 + sz2;
(*_AkA[i])[1][2] = dx*dy - sx*sy;
(*_AkA[i])[1][3] = dx*dz - sx*sz;
(*_AkA[i])[2][0] = (*_AkA[i])[0][2];
(*_AkA[i])[2][1] = (*_AkA[i])[1][2];
(*_AkA[i])[2][2] = sx2 + dy2 + sz2;
(*_AkA[i])[2][3] = dy*dz - sy*sz;
(*_AkA[i])[3][0] = (*_AkA[i])[0][3];
(*_AkA[i])[3][1] = (*_AkA[i])[1][3];
(*_AkA[i])[3][2] = (*_AkA[i])[2][3];
(*_AkA[i])[3][3] = sx2 + sy2 + dz2;
// Rotate normals
x = _refMap[i].normal.x;
y = _refMap[i].normal.y;
z = _refMap[i].normal.z;
_refMap[i].normal.x = _refRotMat[0][0]*x + _refRotMat[1][0]*y + _refRotMat[2][0]*z;
_refMap[i].normal.y = _refRotMat[0][1]*x + _refRotMat[1][1]*y + _refRotMat[2][1]*z;
_refMap[i].normal.z = _refRotMat[0][2]*x + _refRotMat[1][2]*y + _refRotMat[2][2]*z;
x = _dbMap[i].normal.x;
y = _dbMap[i].normal.y;
z = _dbMap[i].normal.z;
_dbMap[i].normal.x = _dbRotMat[0][0]*x + _dbRotMat[1][0]*y + _dbRotMat[2][0]*z;
_dbMap[i].normal.y = _dbRotMat[0][1]*x + _dbRotMat[1][1]*y + _dbRotMat[2][1]*z;
_dbMap[i].normal.z = _dbRotMat[0][2]*x + _dbRotMat[1][2]*y + _dbRotMat[2][2]*z;
}
return;
}
int transform(const char * source, const char * destination, const char * output){
char src_pts_name[256];
char dest_pts_name[256];
char out_param_name[256];
int n=3;
int m=0;
int m2=0;
int k,l;
double **src_mat=NULL;
double **dest_mat=NULL;
double **dest_mat_T=NULL;
double **src_mat_T=NULL;
double **E_mat=NULL;
double **C_mat=NULL;
double **C_mat_interm=NULL;
double **D_mat_interm=NULL;
double **P_mat=NULL;
double *D_vec=NULL;
double *T_vec=NULL;
double *one_vec=NULL;
double **D_mat=NULL;
double **Q_mat=NULL;
double **P_mat_T=NULL;
double **R_mat=NULL;
double trace1=0.0;
double trace2=0.0;
double scal=0.0;
double ppm=0.0;
FILE *outfile;
printf("\n*******************************\n");
printf( "* helmparms3d v%1.2f *\n",VERS);
printf( "* (c) U. Niethammer 2011 *\n");
printf( "* http://helmparms3d.sf.net *\n");
printf( "*******************************\n");
memset(src_pts_name,0,sizeof(src_pts_name));
memset(dest_pts_name,0,sizeof(dest_pts_name));
memset(out_param_name,0,sizeof(out_param_name));
strcpy(src_pts_name, source);
strcpy(dest_pts_name, destination);
strcpy(out_param_name, output);
m=get_m_size(src_pts_name);
m2=get_m_size(dest_pts_name);
if(m2!=m){
printf("Error, number of source and destination points is not equal!\n");
}
else
{
src_mat=matrix(m,m, src_mat);
dest_mat=matrix(m,m, dest_mat);
read_points(src_pts_name, src_mat);
read_points(dest_pts_name, dest_mat);
D_vec=vector(n, D_vec);
E_mat=matrix(m, m, E_mat);
P_mat=matrix(m, m, P_mat);
D_mat=matrix(m, m, D_mat);
Q_mat=matrix(m, m, Q_mat);
P_mat_T=matrix(m, m, P_mat_T);
R_mat=matrix(m, m, R_mat);
dest_mat_T=matrix(m, m, dest_mat_T);
C_mat=matrix(m, m, C_mat);
C_mat_interm=matrix(m, m, C_mat_interm);
src_mat_T=matrix(m, m, src_mat_T);
D_mat_interm=matrix(m, m, D_mat_interm);
transpose_matrix(m, m, dest_mat, dest_mat_T);
if(debug)printf("%s_T:\n",dest_pts_name);
if(debug)plot_matrix(stdout, n, m, dest_mat_T);
for(k=0;k<m;k++){
for(l=0;l<m;l++){
if(k!=l){
E_mat[k][l]=-1.0/(double)m;
}
else{
E_mat[k][l]=1.0-1.0/(double)m;
}
}
}
if(debug)printf("E:\n");
if(debug)plot_matrix(stdout, m, m, E_mat);
if(debug)printf("dest_mat_T:\n");
if(debug)plot_matrix(stdout, n, m, dest_mat_T);
matmult(dest_mat_T, m, m, E_mat, m, m, C_mat_interm, m, n);
if(debug)printf("C_interm:\n");
if(debug)plot_matrix(stdout, n, m, C_mat_interm);
matmult(C_mat_interm, n, m, src_mat, m, n, C_mat, n, n);
if(debug)printf("C:\n");
if(debug)plot_matrix(stdout, n, n, C_mat);
copy_matrix(n,n,C_mat,P_mat);
if(debug)printf("P:\n");
if(debug)plot_matrix(stdout, n, n, P_mat);
//Given matrix C[m][n], m>=n, using svd decomposition C = P D Q' to get P[m][n], diag D[n] and Q[n][n].
svd(n, n, C_mat, P_mat, D_vec, Q_mat);
transpose_matrix(n, n, P_mat, P_mat_T);
if(debug)printf("P\n");
if(debug)plot_matrix(stdout, n, n, P_mat);
if(debug)printf("P_T\n");
if(debug)plot_matrix(stdout, n, n, P_mat_T);
if(debug)printf("D_vec\n");
if(debug)plot_vector(stdout, n, D_vec);
for(k=0;k<n;k++){
for(l=0;l<n;l++){
D_mat[k][l]=0.0;
D_mat[l][l]=D_vec[l];
}
}
if(debug)printf("D\n");
if(debug)plot_matrix(stdout, n, n, D_mat);
matmult(Q_mat, n, n, P_mat_T, n, n, R_mat, n, n);
if(debug)printf("R_trans:\n");
if(debug)plot_matrix(stdout, n, n, R_mat);
matmult(C_mat, m, n, R_mat, n, m, C_mat_interm, m, n);
if(debug)printf("C_interm:\n");
if(debug)plot_matrix(stdout, n, n, C_mat_interm);
trace1=trace(n,n,C_mat_interm);
if(debug)printf("\ntra=%lf\n\n",trace1);
transpose_matrix(m, m, src_mat, src_mat_T);
if(debug)printf("%s_T:\n",src_pts_name);
if(debug)plot_matrix(stdout, n, m, src_mat_T);
init_matrix(m,m,C_mat);
init_matrix(m,m,C_mat_interm);
matmult(src_mat_T, m, m, E_mat, m, m, C_mat_interm, n, n);
if(debug)printf("C_interm:\n");
if(debug)plot_matrix(stdout, n, m, C_mat_interm);
matmult(C_mat_interm, n, m, src_mat, m, n, C_mat, n, n);
if(debug)printf("C:\n");
if(debug)plot_matrix(stdout, n, n, C_mat);
trace2=trace(n,n,C_mat);
if(debug)printf("\ntra=%lf\n\n",trace2);
scal=trace1/trace2;
ppm=scal-1.0;
if(debug)printf("\nscal = %10.10lf\nscal = %10.10lf ppm\n\n",scal, ppm);
init_matrix(m,m,C_mat);
init_matrix(m,m,C_mat_interm);
matmult(src_mat, m, n, R_mat, n,m, D_mat_interm, m, n);
if(debug)printf("C_mat_interm:\n");
if(debug)plot_matrix(stdout, m, n, D_mat_interm);
scal_matrix(m, n, scal, D_mat_interm, C_mat_interm);
if(debug)printf("C_mat_interm:\n");
if(debug)plot_matrix(stdout, m, n, C_mat_interm);
subtract_matrix(m, n, dest_mat, C_mat_interm, D_mat_interm);
if(debug)plot_matrix(stdout, m, n, D_mat_interm);
scal_matrix(m, n, 1.0/m, D_mat_interm, C_mat_interm);
if(debug)plot_matrix(stdout, m, n, C_mat_interm);
init_matrix(m,m,src_mat_T);
transpose_matrix(m, m, C_mat_interm, src_mat_T);
if(debug)plot_matrix(stdout, n, m, src_mat_T);
T_vec=vector(m, T_vec);
one_vec=vector(m, one_vec);
for(k=0;k<m;k++){
one_vec[k]=1.0;
}
matrix_multiply(n, m, src_mat_T, one_vec, T_vec);
if(debug)printf("T:\n");
if(debug)plot_vector(stdout, 3, T_vec);
outfile = fopen(out_param_name, "w");
if(outfile == NULL){
printf("Error writing %s\r\n",out_param_name);
exit(-1);
}
init_matrix(m,m,src_mat_T);
transpose_matrix(m, m, R_mat, src_mat_T);
plot_matrix(outfile, n, n, src_mat_T);
printf("R =\n");fflush(stdout);
plot_matrix(stdout, n, n, src_mat_T);
printf("\n");fflush(stdout);
plot_vector(outfile, 3, T_vec);
printf("T =\n");fflush(stdout);
plot_vector(stdout, 3, T_vec);
printf("\n");fflush(stdout);
fprintf(outfile, "%10.10lf\n", scal);
printf("s = %10.10lf (= %10.10lf ppm)\n\n",scal, ppm);fflush(stdout);
fclose(outfile);
freevector(D_vec);
freevector(T_vec);
freevector(one_vec);
freematrix(m, src_mat);
freematrix(m, dest_mat);
freematrix(m, E_mat);
freematrix(m, P_mat);
freematrix(m, D_mat);
freematrix(m, Q_mat);
freematrix(m, P_mat_T);
freematrix(m, R_mat);
freematrix(m, dest_mat_T);
freematrix(m, C_mat);
freematrix(m, C_mat_interm);
freematrix(m, src_mat_T);
freematrix(m, D_mat_interm);
printf("\n...done\n");
}
}
void LocalLinearLeastSquaresExtrapolator::extrapolateElement(
std::size_t const element_index,
const unsigned num_components,
ExtrapolatableElementCollection const& extrapolatables,
const double t,
GlobalVector const& current_solution,
LocalToGlobalIndexMap const& dof_table,
GlobalVector& counts)
{
auto const& integration_point_values =
extrapolatables.getIntegrationPointValues(
element_index, t, current_solution, dof_table,
_integration_point_values_cache);
auto const& N_0 = extrapolatables.getShapeMatrix(element_index, 0);
auto const num_nodes = static_cast<unsigned>(N_0.cols());
auto const num_values =
static_cast<unsigned>(integration_point_values.size());
if (num_values % num_components != 0)
OGS_FATAL(
"The number of computed integration point values is not divisable "
"by the number of num_components. Maybe the computed property is "
"not a %d-component vector for each integration point.",
num_components);
// number of integration points in the element
const auto num_int_pts = num_values / num_components;
if (num_int_pts < num_nodes)
OGS_FATAL(
"Least squares is not possible if there are more nodes than"
"integration points.");
auto const pair_it_inserted = _qr_decomposition_cache.emplace(
std::make_pair(num_nodes, num_int_pts), CachedData{});
auto& cached_data = pair_it_inserted.first->second;
if (pair_it_inserted.second)
{
DBUG("Computing new singular value decomposition");
// interpolation_matrix * nodal_values = integration_point_values
// We are going to pseudo-invert this relation now using singular value
// decomposition.
auto& interpolation_matrix = cached_data.A;
interpolation_matrix.resize(num_int_pts, num_nodes);
interpolation_matrix.row(0) = N_0;
for (unsigned int_pt = 1; int_pt < num_int_pts; ++int_pt)
{
auto const& shp_mat =
extrapolatables.getShapeMatrix(element_index, int_pt);
assert(shp_mat.cols() == num_nodes);
// copy shape matrix to extrapolation matrix row-wise
interpolation_matrix.row(int_pt) = shp_mat;
}
// JacobiSVD is extremely reliable, but fast only for small matrices.
// But we usually have small matrices and we don't compute very often.
// Cf.
// http://eigen.tuxfamily.org/dox/group__TopicLinearAlgebraDecompositions.html
//
// Decomposes interpolation_matrix = U S V^T.
Eigen::JacobiSVD<Eigen::MatrixXd> svd(
interpolation_matrix, Eigen::ComputeThinU | Eigen::ComputeThinV);
auto const& S = svd.singularValues();
auto const& U = svd.matrixU();
auto const& V = svd.matrixV();
// Compute and save the pseudo inverse V * S^{-1} * U^T.
auto const rank = svd.rank();
assert(rank == num_nodes);
// cf. http://eigen.tuxfamily.org/dox/JacobiSVD_8h_source.html
cached_data.A_pinv.noalias() = V.leftCols(rank) *
S.head(rank).asDiagonal().inverse() *
U.leftCols(rank).transpose();
}
else if (cached_data.A.row(0) != N_0)
{
OGS_FATAL("The cached and the passed shapematrices differ.");
}
auto const& global_indices =
_dof_table_single_component(element_index, 0).rows;
if (num_components == 1)
{
auto const integration_point_values_vec =
MathLib::toVector(integration_point_values);
// Apply the pre-computed pseudo-inverse.
Eigen::VectorXd const nodal_values =
cached_data.A_pinv * integration_point_values_vec;
// TODO does that give rise to PETSc problems? E.g., writing to ghost
// nodes? Furthermore: Is ghost nodes communication necessary for PETSc?
_nodal_values->add(global_indices, nodal_values);
counts.add(global_indices,
std::vector<double>(global_indices.size(), 1.0));
}
else
{
auto const integration_point_values_mat = MathLib::toMatrix(
integration_point_values, num_components, num_int_pts);
// Apply the pre-computed pseudo-inverse.
Eigen::MatrixXd const nodal_values =
cached_data.A_pinv * integration_point_values_mat.transpose();
std::vector<GlobalIndexType> indices;
indices.reserve(num_components * global_indices.size());
// _nodal_values is ordered location-wise
for (unsigned comp = 0; comp < num_components; ++comp)
{
for (auto i : global_indices)
{
indices.push_back(num_components * i + comp);
}
}
// Nodal_values are passed as a raw pointer, because PETScVector and
// EigenVector implementations differ slightly.
_nodal_values->add(indices, nodal_values.data());
counts.add(indices, std::vector<double>(indices.size(), 1.0));
}
}
int main()
{
//Specify data directory
//const std::string dataDirectory = "/Users/ervislilaj/Desktop/data/SimudPuteh";
const std::string dataDirectory = "/Users/ervislilaj/Desktop/data/SmallCaveDownsampled";
//Calculate paths
const std::string outputDirectory = dataDirectory + "/output";
const std::string offFile = dataDirectory + "/model.off";
const std::string skeletonFile = dataDirectory + "/model.skel";
const std::string segmentationFile = dataDirectory + "/segmentation.seg";
const std::string segmentationFile2 = dataDirectory + "/segmentation2.seg";
//Read files
typedef std::vector<Triangle> TriangleList;
std::vector<Eigen::Vector3f> vertices;
std::vector<Eigen::Vector3f> newVertices /*(vertices.size()*2)*/;
TriangleList triangles;
std::vector<Eigen::Vector3f> border;
std::vector<CurveSkeleton::Vertex> skeletonVertices;
std::vector<Vertex> gang;
std::vector<Eigen::Vector3f> borderVertices;
std::vector<Eigen::Vector3f> intersectionVertexForTriangulation;
std::vector<IndexedTriangle> triIndices;
std::vector<IndexedTriangle> borderIndices;
std::vector<IndexedTriangle> labelIndices;
std::vector<IndexedTriangle> leerIndices;
std::vector<Eigen::Vector3f> verwe;
std::vector<int32_t> segmentation;
std::vector<int32_t> segmentation2;
std::vector<double> doubleSeg;
std::vector<int> labels;
//step & iteration parameter
int iter = 490;
double step = 0.001;
std::cout << "Loading mesh file..." << std::endl;
ReadOff(offFile, vertices, triangles, triIndices);
std::cout << "Loading skeleton..." << std::endl;
CurveSkeleton* skeleton = LoadCurveSkeleton(skeletonFile.c_str());
std::cout << "Loading segmentation..." << std::endl;
ReadSegmentation(segmentationFile2, segmentation2, skeleton);
//copy
std::vector<Vertex> vec2;
std::cout << "Calculating inverse correspondences..." << std::endl;
//Calculate inverse correspondences
std::vector<unsigned int> meshVertexCorrespondsTo(vertices.size());
int iVert = -1;
for (auto& vert : skeleton->vertices)
{
++iVert;
for (auto c : vert.correspondingOriginalVertices)
{
meshVertexCorrespondsTo[c] = iVert;
}
}
for (int i = 0; i < segmentation2.size(); ++i) {
doubleSeg.push_back(segmentation2[i]);
}
for (int i = 0; i < vertices.size(); ++i) {
Vertex* new_v = new Vertex();
new_v->p = vertices[i];
new_v->c = doubleSeg[meshVertexCorrespondsTo[i]] + 0.5;
newVertices.push_back(vertices[i]);
vec2.push_back(new_v[0]);
}
cgv::math::union_find ufTri((int)vertices.size());
borderIndices.clear();
labels.clear();
labelIndices.clear();
//set neighbors id
for (int i = 0; i < triIndices.size(); ++i)
{
for (int j = 0; j < triIndices.size(); ++j)
{
if(i==j);
else{
if (((triIndices[j].i[0] == triIndices[i].i[0]) && (triIndices[j].i[1] == triIndices[i].i[1])) ||
((triIndices[j].i[1] == triIndices[i].i[0]) && (triIndices[j].i[0] == triIndices[i].i[1])) ||
((triIndices[j].i[1] == triIndices[i].i[0]) && (triIndices[j].i[2] == triIndices[i].i[1])) ||
((triIndices[j].i[2] == triIndices[i].i[0]) && (triIndices[j].i[1] == triIndices[i].i[1])) ||
((triIndices[j].i[2] == triIndices[i].i[0]) && (triIndices[j].i[0] == triIndices[i].i[1])) ||
((triIndices[j].i[0] == triIndices[i].i[0]) && (triIndices[j].i[2] == triIndices[i].i[1])))
{
// neighbor 1
triIndices[i].n[0] = &triIndices[j];
}
if (((triIndices[j].i[0] == triIndices[i].i[1]) && (triIndices[j].i[1] == triIndices[i].i[2])) ||
((triIndices[j].i[1] == triIndices[i].i[1]) && (triIndices[j].i[0] == triIndices[i].i[2])) ||
((triIndices[j].i[1] == triIndices[i].i[1]) && (triIndices[j].i[2] == triIndices[i].i[2])) ||
((triIndices[j].i[2] == triIndices[i].i[1]) && (triIndices[j].i[1] == triIndices[i].i[2])) ||
((triIndices[j].i[2] == triIndices[i].i[1]) && (triIndices[j].i[0] == triIndices[i].i[2])) ||
((triIndices[j].i[0] == triIndices[i].i[1]) && (triIndices[j].i[2] == triIndices[i].i[2])))
{
// neighbor 2
triIndices[i].n[1] = &triIndices[j];
}
if (((triIndices[j].i[0] == triIndices[i].i[2]) && (triIndices[j].i[1] == triIndices[i].i[0])) ||
((triIndices[j].i[1] == triIndices[i].i[2]) && (triIndices[j].i[0] == triIndices[i].i[0])) ||
((triIndices[j].i[1] == triIndices[i].i[2]) && (triIndices[j].i[2] == triIndices[i].i[0])) ||
((triIndices[j].i[2] == triIndices[i].i[2]) && (triIndices[j].i[1] == triIndices[i].i[0])) ||
((triIndices[j].i[2] == triIndices[i].i[2]) && (triIndices[j].i[0] == triIndices[i].i[0])) ||
((triIndices[j].i[0] == triIndices[i].i[2]) && (triIndices[j].i[2] == triIndices[i].i[0])))
{
// neighbor 3
triIndices[i].n[2] = &triIndices[j];
}
}
}
}
/*
std::vector<IndexedTriangle> toShow;
for(int i = 0; i < triIndices.size(); i += 50)
{
toShow.push_back(triIndices[i]);
toShow.push_back(*triIndices[i].n[0]);
toShow.push_back(*triIndices[i].n[1]);
toShow.push_back(*triIndices[i].n[2]);
}
*/
borderIndices.clear();
labels.clear();
labelIndices.clear();
for (int i = 0; i < vec2.size(); ++i) //für jeden Vertex
{
for (int j = 0; j < triIndices.size(); ++j) //für jedes Dreieck
{
if ((i == triIndices[j].i[0] || i == triIndices[j].i[1] || i == triIndices[j].i[2]))
{
vec2[i].NeigborTri.push_back(j);
}
}
}
int nrOptimization = 0;
double tmpLengthIter = 0;
double lengthIter = 0;
bool firstTimeIter = false;
do
{
std::ofstream fout("iter/50iter" + std::to_string(nrOptimization) + ".xyz", std::ofstream::out);
if(!firstTimeIter)
tmpLengthIter = 99999999;
tmpLengthIter = lengthIter;
lengthIter = 0;
// 1 richtung
for (int i = 0; i < vec2.size(); ++i) //für jeden Vertex
{
double inzidenzEdgesLength = 0;
double tmp = 0;
int index = 0;
do {
Edge e;
tmp = inzidenzEdgesLength;
if (index == 0)
tmp = 10000;
inzidenzEdgesLength = 0;
for (int j = 0; j < vec2[i].NeigborTri.size(); ++j) //für jedes Dreieck
{
triIndices[vec2[i].NeigborTri[j]].visible = true;
if ((i == triIndices[vec2[i].NeigborTri[j]].i[0] || i == triIndices[vec2[i].NeigborTri[j]].i[1] || i == triIndices[vec2[i].NeigborTri[j]].i[2]))
{
//Kanten rechnen sowie deren länge
if (ComputePointsBetweenEdges_v1(vec2, triIndices[vec2[i].NeigborTri[j]], e))
{
e.calculateLength();
inzidenzEdgesLength += e.length;
}
}
}
index++;
vec2[i].c += step;
} while (tmp > inzidenzEdgesLength );
vec2[i].c -= step;
vec2[i].edgeLength = inzidenzEdgesLength;
}
// 2 richtung
for (int i = 0; i < vec2.size(); ++i) //für jeden Vertex
{
double inzidenzEdgesLength = 0;
double tmp = 0;
int index = 0;
do {
Edge e;
tmp = inzidenzEdgesLength;
if (index == 0)
tmp = vec2[i].edgeLength;
inzidenzEdgesLength = 0;
for (int j = 0; j < vec2[i].NeigborTri.size(); ++j) //für jedes Dreieck
{
triIndices[vec2[i].NeigborTri[j]].visible = true;
if ((i == triIndices[vec2[i].NeigborTri[j]].i[0] || i == triIndices[vec2[i].NeigborTri[j]].i[1] || i == triIndices[vec2[i].NeigborTri[j]].i[2]))
{
//Kanten rechnen sowie deren länge
if (ComputePointsBetweenEdges_v1(vec2, triIndices[vec2[i].NeigborTri[j]], e))
{
e.calculateLength();
inzidenzEdgesLength += e.length;
}
}
}
//edgeLengthBlock = inzidenzEdgesLength
index++;
vec2[i].c -= step;
lengthIter += inzidenzEdgesLength;
} while (tmp > inzidenzEdgesLength );
vec2[i].c += step;
}
for (int j = 0; j < triIndices.size(); ++j) //für jedes Dreieck
{
Edge e2;
if (ComputePointsBetweenEdges_v1(vec2, triIndices[j], e2)) {
fout << e2.a.x() << " " << e2.a.y() << " " << e2.a.z() << std::endl;
fout << e2.b.x() << " " << e2.b.y() << " " << e2.b.z() << std::endl;
}
}
nrOptimization++;
fout.close();
std::cout << nrOptimization << std::endl;
std::cout << lengthIter << std::endl;
} while ( (nrOptimization < iter ) && ((tmpLengthIter >= lengthIter) || (nrOptimization < 450)) );
/*End Optimization*/
borderVertices.clear();
for (int j = 0; j < triIndices.size(); ++j) //für jedes Dreieck
{
triIndices[j].iD = j;
Edge e2;
if (ComputePointsBetweenEdges_v1(vec2, triIndices[j], e2)) {
borderVertices.push_back(e2.a);
borderVertices.push_back(e2.b);
}
}
// übergang erkennen
for (int i = 0; i < triIndices.size(); ++i) {
auto v0 = vec2.begin() + (triIndices[i].i[0]);
auto v1 = vec2.begin() + (triIndices[i].i[1]);
auto v2 = vec2.begin() + (triIndices[i].i[2]);
double first = v0[0].c;
double second = v1[0].c;
double third = v2[0].c;
if ((first >= 0.f && second < 0.f) || (first < 0.f && second >= 0.f) || (third >= 0.f && second < 0.f) || (third < 0.f && second >= 0.f) || (third >= 0.f && first < 0.f) || (third < 0.f && first >= 0.f)) {
IndexedTriangle t;
t.i[0] = triIndices[i].i[0];
t.i[1] = triIndices[i].i[1];
t.i[2] = triIndices[i].i[2];
t.iD = triIndices[i].iD;
t.n[0] = triIndices[i].n[0];
t.n[1] = triIndices[i].n[1];
t.n[2] = triIndices[i].n[2];
borderIndices.push_back(t);
}
}
//schallen unite
for (int i = 0; i < borderIndices.size(); ++i)
for (int j = 0; j < borderIndices.size(); ++j)
{
if ((borderIndices[i].i[0] == borderIndices[j].i[0]) || (borderIndices[i].i[0] == borderIndices[j].i[1]) || (borderIndices[i].i[0] == borderIndices[j].i[2]))
{
ufTri.unite(i, j);
}
if ((borderIndices[i].i[1] == borderIndices[j].i[0]) || (borderIndices[i].i[1] == borderIndices[j].i[1]) || (borderIndices[i].i[1] == borderIndices[j].i[2]))
{
ufTri.unite(i, j);
}
if ((borderIndices[i].i[2] == borderIndices[j].i[0]) || (borderIndices[i].i[2] == borderIndices[j].i[1]) || (borderIndices[i].i[2] == borderIndices[j].i[2]))
{
ufTri.unite(i, j);
}
}
std::map<int, std::vector<IndexedTriangle>> labelmain;
// in LabelIndices 1 eiziger gang speichern
for (int i = 0; i < borderIndices.size(); ++i)
{
if (ufTri.num_in_set(ufTri.find(i)) <= 1);
else {
if (std::find(labels.begin(), labels.end(), ufTri.find(i)) != labels.end());
else {
labels.push_back(ufTri.find(i));
}
}
if (labels.size() != 0)
{
int key = ufTri.find(i);
labelmain[key].push_back(borderIndices[i]);
}
}
std::vector<IndexedTriangle> newTriangles;
int offset = (int)vec2.size();
for (int label : labels)
{
labelIndices = labelmain[label];
std::vector<Eigen::Vector3f> labelVertices;
std::vector<Eigen::Vector3f> halleVertices;
labelVertices.clear();
Edge labelEdge;
for (int i = 0; i < labelIndices.size(); ++i)
{
if (ComputePointsBetweenEdges_v1(vec2, labelIndices[i], labelEdge))
{
labelVertices.push_back(labelEdge.a);
labelVertices.push_back(labelEdge.b);
}
}
/*Plane Fitting with SVD */
Eigen::Vector3f vor_c(0, 0, 0);
Eigen::Vector3f c(0, 0, 0);
Eigen::MatrixXf matA(3, labelVertices.size());
// calculate c
for (int i = 0; i < labelVertices.size(); ++i)
{
vor_c += labelVertices[i];
}
c = vor_c / labelVertices.size();
//fill the Matrix
for (int i = 0; i < labelVertices.size(); i++)
{
float xB, yB, zB;
xB = labelVertices[i].x() - c.x();
yB = labelVertices[i].y() - c.y();
zB = labelVertices[i].z() - c.z();
matA.col(i) << xB, yB, zB;
}
Eigen::JacobiSVD<Eigen::MatrixXf> svd(matA, Eigen::ComputeThinU | Eigen::ComputeThinV);
Eigen::Vector3f planeNormal(0, 0, 0);
planeNormal = svd.matrixU().col(2);
float d = -(planeNormal.x()*c.x() + planeNormal.y()*c.y() + planeNormal.z()*c.z());
/*Begin Normal direction of the plane*/
std::vector<Eigen::Vector3f> halleVerticesSidePLane;
std::vector<double> disthalleVerticesSidePLane;
int indexHalleV;
Eigen::Vector3f halle_cVector;
//alle halle knoten hinzufügen
for(int i = 0; i < labelIndices.size(); ++i)
{
if(vec2[labelIndices[i].i[0]].c >= 0)
halleVerticesSidePLane.push_back(vec2[labelIndices[i].i[0]].p);
if(vec2[labelIndices[i].i[1]].c >= 0)
halleVerticesSidePLane.push_back(vec2[labelIndices[i].i[1]].p);
if(vec2[labelIndices[i].i[2]].c >= 0)
halleVerticesSidePLane.push_back(vec2[labelIndices[i].i[2]].p);
}
for(int i = 0; i < halleVerticesSidePLane.size(); ++i)
{
disthalleVerticesSidePLane.push_back( Dist2Plane(halleVerticesSidePLane[i], planeNormal, d));
}
indexHalleV = *std::max_element(disthalleVerticesSidePLane.begin(), disthalleVerticesSidePLane.end());
halle_cVector = halleVerticesSidePLane[indexHalleV] - c;
Eigen::Vector3f diffBetweenPlane_halle_positive = halle_cVector + planeNormal;
Eigen::Vector3f diffBetweenPlane_halle_negative = halle_cVector - planeNormal;
float lengthPositive =sqrt(diffBetweenPlane_halle_positive.x()* diffBetweenPlane_halle_positive.x() + diffBetweenPlane_halle_positive.y()* diffBetweenPlane_halle_positive.y() + diffBetweenPlane_halle_positive.z()* diffBetweenPlane_halle_positive.z() );
float lengthNegative =sqrt(diffBetweenPlane_halle_negative.x()* diffBetweenPlane_halle_negative.x() + diffBetweenPlane_halle_negative.y()* diffBetweenPlane_halle_negative.y() + diffBetweenPlane_halle_negative.z()* diffBetweenPlane_halle_negative.z() );
if (lengthPositive > lengthNegative)
planeNormal = -planeNormal;
else if(lengthPositive <= lengthNegative)
planeNormal = planeNormal;
// ax + bx + cx + d = 0 Plane equation
d = -(planeNormal.x()*c.x() + planeNormal.y()*c.y() + planeNormal.z()*c.z());
/*Begin computing Intersection Points*/
//Point which has the largest distance to c
std::vector<double> MaxDistanceToC;
double radiusMax;
for (int i = 0; i < labelVertices.size(); ++i)
{
MaxDistanceToC.push_back(Dist2Points(c, labelVertices[i]));
}
radiusMax = *std::max_element(MaxDistanceToC.begin(), MaxDistanceToC.end());
//Computing Intersection points
int succesor = offset - 1;
std::vector<Edge> intersectionEdges;
intersectionEdges.clear();
//Label Nachbarn hinzufügen. 8schritte 3^12
// First Triangle Hinzufügen die geschnitten wird und teil von lableindices ist
IndexedTriangle firstTri;
int firstTriID = 0;
bool firstTimeSearch = true;
for (int i = 0; i < labelIndices.size(); ++i){
Eigen::Vector3f aP, bP, cP, intersection1, intersection2;
int aI = 0, bI = 0, cI = 0;
aI = labelIndices[i].i[0];
bI = labelIndices[i].i[1];
cI = labelIndices[i].i[2];
aP = vec2[aI].p;
bP = vec2[bI].p;
cP = vec2[cI].p;
if((SegPlaneIntersection(aP, bP, intersection1, planeNormal, d) || SegPlaneIntersection(aP, cP, intersection2, planeNormal, d) ||
SegPlaneIntersection(bP, cP, intersection2, planeNormal, d)) && firstTimeSearch)
{
firstTri = triIndices[labelIndices[i].iD];
firstTimeSearch = false;
firstTriID = labelIndices[i].iD;
}
}
std::set<int> triIndicesHit ;
int mico = 0;
do{
for (int m = 0; m < 3 ; ++m)
{
Eigen::Vector3f aP, bP, cP, intersection1, intersection2;
IndexedTriangle t1,t2;
//indices
int aI = firstTri.n[m]->i[0];
int bI = firstTri.n[m]->i[1];
int cI = firstTri.n[m]->i[2];
aP = vec2[aI].p;
bP = vec2[bI].p;
cP = vec2[cI].p;
if((SegPlaneIntersection(aP, bP, intersection1, planeNormal, d) || SegPlaneIntersection(aP, cP, intersection2, planeNormal, d) ||
SegPlaneIntersection(bP, cP, intersection2, planeNormal, d)) && (triIndicesHit.find(firstTri.n[m]->iD) == triIndicesHit.end())){
triIndicesHit.insert(firstTri.n[m]->iD);
if (SegPlaneIntersection(aP, bP, intersection1, planeNormal, d) && SegPlaneIntersection(aP, cP, intersection2, planeNormal, d) && (Dist2Plane(aP, planeNormal, d) < 0))
{
newVertices.push_back(intersection1);
newVertices.push_back(intersection2);
t1.i[0] = aI;
t1.i[2] = succesor + 2;
t1.i[1] = succesor + 1;
t1.visible = true;
newTriangles.push_back(t1);
firstTri = *firstTri.n[m];
succesor++;
succesor++;
}
if (SegPlaneIntersection(bP, cP, intersection1, planeNormal, d) && SegPlaneIntersection(bP, aP, intersection2, planeNormal, d) && (Dist2Plane(bP, planeNormal, d) < 0))
{
newVertices.push_back(intersection1);
newVertices.push_back(intersection2);
t1.i[0] = bI;
t1.i[2] = succesor + 2;
t1.i[1] = succesor + 1;
t1.visible = true;
newTriangles.push_back(t1);
firstTri = *firstTri.n[m];
succesor++;
succesor++;
}
if (SegPlaneIntersection(cP, aP, intersection1, planeNormal, d) && SegPlaneIntersection(cP, bP, intersection2, planeNormal, d) && (Dist2Plane(cP, planeNormal, d) < 0))
{
newVertices.push_back(intersection1);
newVertices.push_back(intersection2);
t1.i[0] = cI;
t1.i[2] = succesor + 2;
t1.i[1] = succesor + 1;
t1.visible = true;
newTriangles.push_back(t1);
firstTri = *firstTri.n[m];
succesor++;
succesor++;
}
if (SegPlaneIntersection(aP, bP, intersection1, planeNormal, d) && SegPlaneIntersection(aP, cP, intersection2, planeNormal, d) && (Dist2Plane(bP, planeNormal, d) < 0) && (Dist2Plane(cP, planeNormal, d) < 0))
{
newVertices.push_back(intersection1);
newVertices.push_back(intersection2);
t1.i[0] = bI;
t1.i[2] = succesor + 1;
t1.i[1] = succesor + 2;
t2.i[0] = cI;
t2.i[2] = bI;
t2.i[1] = succesor + 2;
t1.visible = true;
t2.visible = true;
newTriangles.push_back(t1);
newTriangles.push_back(t2);
firstTri = *firstTri.n[m];
succesor++;
succesor++;
}
if (SegPlaneIntersection(bP, cP, intersection1, planeNormal, d) && SegPlaneIntersection(bP, aP, intersection2, planeNormal, d) && (Dist2Plane(cP, planeNormal, d) < 0) && (Dist2Plane(aP, planeNormal, d) < 0))
{
newVertices.push_back(intersection1);
newVertices.push_back(intersection2);
t1.i[0] = cI;
t1.i[2] = succesor + 1;
t1.i[1] = succesor + 2;
t2.i[0] = aI;
t2.i[2] = cI;
t2.i[1] = succesor + 2;
t1.visible = true;
t2.visible = true;
newTriangles.push_back(t1);
newTriangles.push_back(t2);
firstTri = *firstTri.n[m];
succesor++;
succesor++;
}
if (SegPlaneIntersection(cP, aP, intersection1, planeNormal, d) && SegPlaneIntersection(cP, bP, intersection2, planeNormal, d) && (Dist2Plane(aP, planeNormal, d) < 0) && (Dist2Plane(bP, planeNormal, d) < 0))
{
newVertices.push_back(intersection1);
newVertices.push_back(intersection2);
t1.i[0] = aI;
t1.i[2] = succesor + 1;
t1.i[1] = succesor + 2;
t2.i[0] = bI;
t2.i[2] = aI;
t2.i[1] = succesor + 2;
t1.visible = true;
t2.visible = true;
newTriangles.push_back(t1);
newTriangles.push_back(t2);
firstTri = *firstTri.n[m];
succesor++;
succesor++;
}
}
}
mico ++;
}while (mico < 1000000);
//nachbarn von Schnitt hinzufügen
std::set<int> extendedLabelIndices ;
for (int i = 0; i < triIndices.size(); ++i)
{
const bool is_in = triIndicesHit.find(triIndices[i].iD) != triIndicesHit.end();
if(is_in){
extendedLabelIndices.insert(triIndices[i].iD);
for(int j = 0; j < 3; ++j){
extendedLabelIndices.insert(triIndices[i].n[j]->iD);
for(int k = 0; k < 3; ++k){
extendedLabelIndices.insert(triIndices[i].n[j]->n[k]->iD);
for(int l = 0; l < 3; ++l){
extendedLabelIndices.insert(triIndices[i].n[j]->n[k]->n[l]->iD);
for(int f = 0; f < 3; ++f){
extendedLabelIndices.insert(triIndices[i].n[j]->n[k]->n[l]->n[f]->iD);
for(int s = 0; s < 3; ++s){
extendedLabelIndices.insert(triIndices[i].n[j]->n[k]->n[l]->n[f]->n[s]->iD);
for(int r = 0; r < 3; ++r){
extendedLabelIndices.insert(triIndices[i].n[j]->n[k]->n[l]->n[f]->n[s]->n[r]->iD);
}
}
}
}
}
}
}
}
/* for (int i = 0; i < triIndices.size(); ++i)
{
int a = triIndices[i].i[0],b = triIndices[i].i[1] ,c = triIndices[i].i[2] ;
if(( vec2[a].c >= 0 )&& ( vec2[b].c >= 0 ) && ( vec2[c].c >= 0 ))
{
triIndices[i].visible = true;
}
}*/
for (int i = 0; i < triIndices.size(); ++i)
{
//positions
Eigen::Vector3f aP, bP, cP, intersection1, intersection2;
//distance to middlepoint
float dista, distb, distc;
//indices
int aI = 0, bI = 0, cI = 0;
aI = triIndices[i].i[0];
bI = triIndices[i].i[1];
cI = triIndices[i].i[2];
aP = vec2[aI].p;
bP = vec2[bI].p;
cP = vec2[cI].p;
dista = Dist2Points(c, aP);
distb = Dist2Points(c, bP);
distc = Dist2Points(c, cP);
const bool is_in = extendedLabelIndices.find(triIndices[i].iD) != extendedLabelIndices.end();
if (is_in)
{
if ((Dist2Plane(aP, planeNormal, d) > 0) || (Dist2Plane(bP, planeNormal, d) > 0) || (Dist2Plane(cP, planeNormal, d) > 0))
{
triIndices[i].visible = false;
}
if (((Dist2Plane(aP, planeNormal, d) <= 0) || (Dist2Plane(bP, planeNormal, d) <= 0) || (Dist2Plane(cP, planeNormal, d) <= 0)) && ((Dist2Plane(aP, planeNormal, d) > 10) || (Dist2Plane(bP, planeNormal, d) > 10) || (Dist2Plane(cP, planeNormal, d) > 10)))
{
triIndices[i].visible = true;
}
}
}
//umbrella
Eigen::Vector3f vor_ca(0,0,0), ca(0, 0, 0);
int anz = (int)newVertices.size() - (offset);
for (int j = offset ; j < (int)newVertices.size() ; ++j)
{
vor_ca += newVertices[j];
}
ca = vor_c / anz;
newVertices.push_back(c);
for (int j = offset; j < newVertices.size()-2; j+=2)
{
IndexedTriangle t;
t.i[1] = j;
t.i[2] = j + 1;
t.i[0] = (int)newVertices.size() - 1;
newTriangles.push_back(t);
}
offset = (int)newVertices.size();
/*End computing Intersection Points*/
}
for(auto t : triIndices)
if(t.visible && (vec2[t.i[0]].c >= 0 || vec2[t.i[1]].c >= 0 || vec2[t.i[2]].c >= 0))
newTriangles.push_back(t);
WriteSegmentation(segmentationFile, segmentation2);
ReadSegmentation(segmentationFile, segmentation, skeleton);
//Test output
std::cout << "Writing output file..." << std::endl;
/*int colors[10][3] =
{
{ 166, 206, 227 },
{ 31, 120, 180 },
{ 251, 154, 153 },
{ 227, 26, 28 },
{ 253, 191, 111 },
{ 255, 127, 0 },
{ 202, 178, 214 },
{ 106, 61, 154 },
{ 255, 255, 153 },
{ 177, 89, 40 }
};*/
auto colorFunc = [&](int i, int& r, int& g, int& b)
{
if (doubleSeg[i] == -1)
{
//color for passages
r = 0; g = 175; b = 0;
}
else
if (doubleSeg[i] >= 0)
{
r = 166; g = 175; b = 227;
}
else
if (doubleSeg[i] == 0)
{
r = 0; g = 0; b = 175;
}
};
std::string segmentedMeshFile = outputDirectory + "/segmentedMesh.off";
WriteOff(segmentedMeshFile.c_str(), vertices, triIndices, [&](int i, int& r, int& g, int& b) {colorFunc(meshVertexCorrespondsTo[i], r, g, b); });
std::string segmentedMeshFile2 = outputDirectory + "/borderVertices.off";
WriteOff(segmentedMeshFile2.c_str(), vertices, borderIndices, [&](int i, int& r, int& g, int& b) {colorFunc(meshVertexCorrespondsTo[i], r, g, b); });
std::string segmentedMeshFile3 = outputDirectory + "/schalle.off";
WriteOff(segmentedMeshFile3.c_str(), vertices, borderIndices, [&](int i, int& r, int& g, int& b) {colorFunc(meshVertexCorrespondsTo[i], r, g, b); });
std::string segmentedMeshFile7 = outputDirectory + "/marchingBig.off";
WriteOff(segmentedMeshFile7.c_str(), verwe , leerIndices, [&](int i, int& r, int& g, int& b) { r = 175; g = 0; b = 0; });
std::string segmentedMeshFile8 = outputDirectory + "/new.off";
WriteOff(segmentedMeshFile8.c_str(), newVertices, newTriangles, [&](int i, int& r, int& g, int& b) { r = 200; g = 200; b = 0; });
//system("PAUSE");
}
Foam::label Foam::quadraticFitSnGradData::calcFit
(
const List<point>& C,
const label faci
)
{
vector idir(1,0,0);
vector jdir(0,1,0);
vector kdir(0,0,1);
findFaceDirs(idir, jdir, kdir, mesh(), faci);
scalarList wts(C.size(), scalar(1));
wts[0] = centralWeight_;
wts[1] = centralWeight_;
point p0 = mesh().faceCentres()[faci];
scalar scale = 0;
// calculate the matrix of the polynomial components
scalarRectangularMatrix B(C.size(), minSize_, scalar(0));
for(label ip = 0; ip < C.size(); ip++)
{
const point& p = C[ip];
scalar px = (p - p0)&idir;
scalar py = (p - p0)&jdir;
#ifdef SPHERICAL_GEOMETRY
scalar pz = mag(p) - mag(p0);
#else
scalar pz = (p - p0)&kdir;
#endif
if (ip == 0) scale = max(max(mag(px), mag(py)), mag(pz));
px /= scale;
py /= scale;
pz /= scale;
label is = 0;
B[ip][is++] = wts[0]*wts[ip];
B[ip][is++] = wts[0]*wts[ip]*px;
B[ip][is++] = wts[ip]*sqr(px);
if (dim_ >= 2)
{
B[ip][is++] = wts[ip]*py;
B[ip][is++] = wts[ip]*px*py;
B[ip][is++] = wts[ip]*sqr(py);
}
if (dim_ == 3)
{
B[ip][is++] = wts[ip]*pz;
B[ip][is++] = wts[ip]*px*pz;
//B[ip][is++] = wts[ip]*py*pz;
B[ip][is++] = wts[ip]*sqr(pz);
}
}
// Set the fit
label stencilSize = C.size();
fit_[faci].setSize(stencilSize);
scalarList singVals(minSize_);
label nSVDzeros = 0;
const scalar& deltaCoeff = mesh().deltaCoeffs()[faci];
bool goodFit = false;
for(int iIt = 0; iIt < 10 && !goodFit; iIt++)
{
SVD svd(B, SMALL);
scalar fit0 = wts[0]*wts[0]*svd.VSinvUt()[1][0]/scale;
scalar fit1 = wts[0]*wts[1]*svd.VSinvUt()[1][1]/scale;
goodFit =
fit0 < 0 && fit1 > 0
&& mag(fit0 + deltaCoeff) < 0.5*deltaCoeff
&& mag(fit1 - deltaCoeff) < 0.5*deltaCoeff;
if (goodFit)
{
fit_[faci][0] = fit0;
fit_[faci][1] = fit1;
for(label i = 2; i < stencilSize; i++)
{
fit_[faci][i] = wts[0]*wts[i]*svd.VSinvUt()[1][i]/scale;
}
singVals = svd.S();
nSVDzeros = svd.nZeros();
}
else // (not good fit so increase weight in the centre and for linear)
{
wts[0] *= 10;
wts[1] *= 10;
for(label i = 0; i < B.n(); i++)
{
B[i][0] *= 10;
B[i][1] *= 10;
}
for(label j = 0; j < B.m(); j++)
{
B[0][j] *= 10;
B[1][j] *= 10;
}
}
}
if (goodFit)
{
// remove the uncorrected snGradScheme coefficients
fit_[faci][0] += deltaCoeff;
fit_[faci][1] -= deltaCoeff;
}
else
{
Pout<< "quadratifFitSnGradData could not fit face " << faci
<< " fit_[faci][0] = " << fit_[faci][0]
<< " fit_[faci][1] = " << fit_[faci][1]
<< " deltaCoeff = " << deltaCoeff << endl;
fit_[faci] = 0;
}
return minSize_ - nSVDzeros;
}
void
computeConsistentRotations_Linf(double const sigma, int const nIterations, int const nViews,
std::vector<Matrix3x3d> const& relativeRotations,
std::vector<std::pair<int, int> > const& viewPairs,
std::vector<Matrix3x3d>& rotations, std::vector<double>& zs)
{
double const gamma = 1.0;
int const nRelPoses = relativeRotations.size();
rotations.resize(nViews);
Matrix3x3d zero3x3d;
makeZeroMatrix(zero3x3d);
Matrix4x4d zeroQuat;
makeZeroMatrix(zeroQuat); zeroQuat[0][0] = 1;
double const denomT = 1.0 / (1.0 + nRelPoses);
vector<double> denomQ(nViews, 1.0); // from the psd constraint
for (int k = 0; k < nRelPoses; ++k)
{
int const i = viewPairs[k].first;
int const j = viewPairs[k].second;
denomQ[i] += 1;
denomQ[j] += 1;
}
for (int i = 0; i < nViews; ++i) denomQ[i] = 1.0 / denomQ[i];
double T = 0.0;
vector<double> T1(nRelPoses);
vector<double> ZT1(nRelPoses, 0.0);
double T2;
double ZT2 = 0;
vector<Matrix4x4d> Q(nViews, zeroQuat);
vector<Matrix4x4d> Q1(nViews, zeroQuat);
vector<Matrix4x4d> ZQ1(nViews, zeroQuat);
vector<Matrix4x4d> Q2i(nRelPoses, zeroQuat);
vector<Matrix4x4d> Q2j(nRelPoses, zeroQuat);
vector<Matrix4x4d> ZQ2i(nRelPoses, zeroQuat);
vector<Matrix4x4d> ZQ2j(nRelPoses, zeroQuat);
vector<Matrix3x3d> A(nRelPoses, zero3x3d);
vector<Matrix3x3d> A1(nRelPoses, zero3x3d);
vector<Matrix3x3d> A2(nRelPoses, zero3x3d);
vector<Matrix3x3d> ZA1(nRelPoses, zero3x3d);
vector<Matrix3x3d> ZA2(nRelPoses, zero3x3d);
for (int iter = 0; iter < nIterations; ++iter)
{
// Convex hull of rotation matrices
for (int i = 0; i < nViews; ++i)
{
Matrix4x4d q = Q[i] + ZQ1[i];
if (i > 0)
projectConvHull_SO3(q);
else
{
makeZeroMatrix(q);
q[0][0] = 1;
}
Q1[i] = q;
addMatricesIP(Q[i] - q, ZQ1[i]);
} // end for (i)
// Shrinkage of T (we want to minimize T)
T2 = std::max(0.0, T + ZT2 - gamma);
ZT2 += T - T2;
// Cone constraint
for (int k = 0; k < nRelPoses; ++k)
{
double t = T + ZT1[k];
Matrix3x3d a = A[k] + ZA1[k];
proxDataResidual_Frobenius(sigma, t, a);
T1[k] = t;
ZT1[k] += T - t;
A1[k] = a;
addMatricesIP(A[k] - a, ZA1[k]);
} // end for (k)
// Enforce linear consistency
for (int k = 0; k < nRelPoses; ++k)
{
int const i = viewPairs[k].first;
int const j = viewPairs[k].second;
Matrix4x4d qi = Q[i] + ZQ2i[k];
Matrix4x4d qj = Q[j] + ZQ2j[k];
Matrix3x3d a = A[k] + ZA2[k];
proxConsistency(relativeRotations[k], qi, qj, a);
Q2i[k] = qi;
Q2j[k] = qj;
A2[k] = a;
addMatricesIP(Q[i] - qi, ZQ2i[k]);
addMatricesIP(Q[j] - qj, ZQ2j[k]);
addMatricesIP(A[k] - a, ZA2[k]);
} // end for (k)
// Averaging of the solutions
for (int i = 0; i < nViews; ++i) Q[i] = Q1[i] - ZQ1[i];
T = T2 - ZT2;
for (int k = 0; k < nRelPoses; ++k) T += T1[k] - ZT1[k];
T *= denomT;
T = std::max(0.0, T);
for (int k = 0; k < nRelPoses; ++k) A[k] = A1[k] - ZA1[k];
for (int k = 0; k < nRelPoses; ++k)
{
int const i = viewPairs[k].first;
int const j = viewPairs[k].second;
addMatricesIP(Q2i[k] - ZQ2i[k], Q[i]);
addMatricesIP(Q2j[k] - ZQ2j[k], Q[j]);
addMatricesIP(A2[k] - ZA2[k], A[k]);
} // end for (k)
for (int i = 0; i < nViews; ++i) scaleMatrixIP(denomQ[i], Q[i]);
for (int k = 0; k < nRelPoses; ++k) scaleMatrixIP(0.5, A[k]);
if ((iter % 500) == 0)
{
cout << "iter: " << iter << " t = " << T << endl;
cout << "T1 = "; displayVector(T1);
cout << "ZT1 = "; displayVector(ZT1);
cout << "T2 = " << T2 << " ZT2 = " << ZT2 << endl;
Matrix<double> ZZ(4, 4);
for (int i = 0; i < nViews; ++i)
{
copyMatrix(Q[i], ZZ);
SVD<double> svd(ZZ);
cout << "Q = "; displayMatrix(ZZ);
cout << "SV = "; displayVector(svd.getSingularValues());
//Matrix3x3d R = getRotationFromQuat(Q[i]);
//cout << "R = "; displayMatrix(R);
} // end for (i)
}
} // end for (iter)
rotations.resize(nViews);
for (int i = 0; i < nViews; ++i)
rotations[i] = getRotationFromQuat(Q[i]);
zs = ZT1;
} // end computeConsistentRotations_Linf()
// Norm: Return largest singular value for M-by-N matrix.
//---------------------------------------------------------
double norm(const DMat& mat)
//---------------------------------------------------------
{
DVec s( svd(mat) );
return s(1);
}
/**
* @brief estimateHomography estimates an homography matrix H between image 1 to image 2
* @param points0 is an array of points computed from image 1.
* @param points1 is an array of points computed from image 2.
* @return It returns the homography matrix H.
*/
PIC_INLINE Eigen::Matrix3d estimateHomography(std::vector< Eigen::Vector2f > &points0,
std::vector< Eigen::Vector2f > &points1)
{
Eigen::Matrix3d H;
if((points0.size() != points1.size()) || (points0.size() < 4)) {
H.setZero();
return H;
}
Eigen::Vector3f transform_0 = ComputeNormalizationTransform(points0);
Eigen::Vector3f transform_1 = ComputeNormalizationTransform(points1);
Eigen::Matrix3d mat_0 = getShiftScaleMatrix(transform_0);
Eigen::Matrix3d mat_1 = getShiftScaleMatrix(transform_1);
int n = int(points0.size());
Eigen::MatrixXd A(n * 2, 9);
//set up the linear system
for(int i = 0; i < n; i++) {
//transform coordinates for increasing stability of the system
Eigen::Vector2f p0 = points0[i];
Eigen::Vector2f p1 = points1[i];
p0[0] = (p0[0] - transform_0[0]) / transform_0[2];
p0[1] = (p0[1] - transform_0[1]) / transform_0[2];
p1[0] = (p1[0] - transform_1[0]) / transform_1[2];
p1[1] = (p1[1] - transform_1[1]) / transform_1[2];
int j = i * 2;
A(j, 0) = 0.0;
A(j, 1) = 0.0;
A(j, 2) = 0.0;
A(j, 3) = p0[0];
A(j, 4) = p0[1];
A(j, 5) = 1.0;
A(j, 6) = -p1[1] * p0[0];
A(j, 7) = -p1[1] * p0[1];
A(j, 8) = -p1[1];
j++;
A(j, 0) = p0[0];
A(j, 1) = p0[1];
A(j, 2) = 1.0;
A(j, 3) = 0.0;
A(j, 4) = 0.0;
A(j, 5) = 0.0;
A(j, 6) = -p1[0] * p0[0];
A(j, 7) = -p1[0] * p0[1];
A(j, 8) = -p1[0];
}
//solve the linear system
Eigen::JacobiSVD< Eigen::MatrixXd > svd(A, Eigen::ComputeFullV);
Eigen::MatrixXd V = svd.matrixV();
n = int(V.cols()) - 1;
//assign and transpose
H(0, 0) = V(0, n);
H(0, 1) = V(1, n);
H(0, 2) = V(2, n);
H(1, 0) = V(3, n);
H(1, 1) = V(4, n);
H(1, 2) = V(5, n);
H(2, 0) = V(6, n);
H(2, 1) = V(7, n);
H(2, 2) = V(8, n);
H = mat_1.inverse() * H * mat_0;
return H / H(2, 2);
}
bool Homography::
decompose()
{
decompositions.clear();
JacobiSVD<MatrixXd> svd(H_c2_from_c1, ComputeThinU | ComputeThinV);
Vector3d singular_values = svd.singularValues();
double d1 = fabs(singular_values[0]); // The paper suggests the square of these (e.g. the evalues of AAT)
double d2 = fabs(singular_values[1]); // should be used, but this is wrong. c.f. Faugeras' book.
double d3 = fabs(singular_values[2]);
Matrix3d U = svd.matrixU();
Matrix3d V = svd.matrixV(); // VT^T
double s = U.determinant() * V.determinant();
double dPrime_PM = d2;
int nCase;
if(d1 != d2 && d2 != d3)
nCase = 1;
else if( d1 == d2 && d2 == d3)
nCase = 3;
else
nCase = 2;
if(nCase != 1)
{
printf("FATAL Homography Initialization: This motion case is not implemented or is degenerate. Try again. ");
return false;
}
double x1_PM;
double x2;
double x3_PM;
// All below deals with the case = 1 case.
// Case 1 implies (d1 != d3)
{ // Eq. 12
x1_PM = sqrt((d1*d1 - d2*d2) / (d1*d1 - d3*d3));
x2 = 0;
x3_PM = sqrt((d2*d2 - d3*d3) / (d1*d1 - d3*d3));
};
double e1[4] = {1.0,-1.0, 1.0,-1.0};
double e3[4] = {1.0, 1.0,-1.0,-1.0};
Vector3d np;
HomographyDecomposition decomp;
// Case 1, d' > 0:
decomp.d = s * dPrime_PM;
for(size_t signs=0; signs<4; signs++)
{
// Eq 13
decomp.R = Matrix3d::Identity();
double dSinTheta = (d1 - d3) * x1_PM * x3_PM * e1[signs] * e3[signs] / d2;
double dCosTheta = (d1 * x3_PM * x3_PM + d3 * x1_PM * x1_PM) / d2;
decomp.R(0,0) = dCosTheta;
decomp.R(0,2) = -dSinTheta;
decomp.R(2,0) = dSinTheta;
decomp.R(2,2) = dCosTheta;
// Eq 14
decomp.t[0] = (d1 - d3) * x1_PM * e1[signs];
decomp.t[1] = 0.0;
decomp.t[2] = (d1 - d3) * -x3_PM * e3[signs];
np[0] = x1_PM * e1[signs];
np[1] = x2;
np[2] = x3_PM * e3[signs];
decomp.n = V * np;
decompositions.push_back(decomp);
}
// Case 1, d' < 0:
decomp.d = s * -dPrime_PM;
for(size_t signs=0; signs<4; signs++)
{
// Eq 15
decomp.R = -1 * Matrix3d::Identity();
double dSinPhi = (d1 + d3) * x1_PM * x3_PM * e1[signs] * e3[signs] / d2;
double dCosPhi = (d3 * x1_PM * x1_PM - d1 * x3_PM * x3_PM) / d2;
decomp.R(0,0) = dCosPhi;
decomp.R(0,2) = dSinPhi;
decomp.R(2,0) = dSinPhi;
decomp.R(2,2) = -dCosPhi;
// Eq 16
decomp.t[0] = (d1 + d3) * x1_PM * e1[signs];
decomp.t[1] = 0.0;
decomp.t[2] = (d1 + d3) * x3_PM * e3[signs];
np[0] = x1_PM * e1[signs];
np[1] = x2;
np[2] = x3_PM * e3[signs];
decomp.n = V * np;
decompositions.push_back(decomp);
}
// Save rotation and translation of the decomposition
for(unsigned int i=0; i<decompositions.size(); i++)
{
Matrix3d R = s * U * decompositions[i].R * V.transpose();
Vector3d t = U * decompositions[i].t;
decompositions[i].T = Sophus::SE3(R, t);
}
return true;
}
void Foam::CentredFitSnGradData<Polynomial>::calcFit
(
scalarList& coeffsi,
const List<point>& C,
const scalar wLin,
const scalar deltaCoeff,
const label facei
)
{
vector idir(1,0,0);
vector jdir(0,1,0);
vector kdir(0,0,1);
this->findFaceDirs(idir, jdir, kdir, facei);
// Setup the point weights
scalarList wts(C.size(), scalar(1));
wts[0] = this->centralWeight();
wts[1] = this->centralWeight();
// Reference point
point p0 = this->mesh().faceCentres()[facei];
// p0 -> p vector in the face-local coordinate system
vector d;
// Local coordinate scaling
scalar scale = 1;
// Matrix of the polynomial components
scalarRectangularMatrix B(C.size(), this->minSize(), scalar(0));
forAll(C, ip)
{
const point& p = C[ip];
const vector p0p = p - p0;
d.x() = p0p & idir;
d.y() = p0p & jdir;
d.z() = p0p & kdir;
if (ip == 0)
{
scale = cmptMax(cmptMag((d)));
}
// Scale the radius vector
d /= scale;
Polynomial::addCoeffs(B[ip], d, wts[ip], this->dim());
}
// Additional weighting for constant and linear terms
for (label i = 0; i < B.m(); i++)
{
B(i, 0) *= wts[0];
B(i, 1) *= wts[0];
}
// Set the fit
label stencilSize = C.size();
coeffsi.setSize(stencilSize);
bool goodFit = false;
for (int iIt = 0; iIt < 8 && !goodFit; iIt++)
{
SVD svd(B, small);
scalarRectangularMatrix invB(svd.VSinvUt());
for (label i=0; i<stencilSize; i++)
{
coeffsi[i] = wts[1]*wts[i]*invB(1, i)/scale;
}
goodFit =
(
mag(wts[0]*wts[0]*invB(0, 0) - wLin)
< this->linearLimitFactor()*wLin)
&& (mag(wts[0]*wts[1]*invB(0, 1) - (1 - wLin)
) < this->linearLimitFactor()*(1 - wLin))
&& coeffsi[0] < 0 && coeffsi[1] > 0
&& mag(coeffsi[0] + deltaCoeff) < 0.5*deltaCoeff
&& mag(coeffsi[1] - deltaCoeff) < 0.5*deltaCoeff;
if (!goodFit)
{
// (not good fit so increase weight in the centre and weight
// for constant and linear terms)
WarningInFunction
<< "Cannot fit face " << facei << " iteration " << iIt
<< " with sum of weights " << sum(coeffsi) << nl
<< " Weights " << coeffsi << nl
<< " Linear weights " << wLin << " " << 1 - wLin << nl
<< " deltaCoeff " << deltaCoeff << nl
<< " sing vals " << svd.S() << nl
<< "Components of goodFit:\n"
<< " wts[0]*wts[0]*invB(0, 0) = "
<< wts[0]*wts[0]*invB(0, 0) << nl
<< " wts[0]*wts[1]*invB(0, 1) = "
<< wts[0]*wts[1]*invB(0, 1)
<< " dim = " << this->dim() << endl;
wts[0] *= 10;
wts[1] *= 10;
for (label j = 0; j < B.n(); j++)
{
B(0, j) *= 10;
B(1, j) *= 10;
}
for (label i = 0; i < B.m(); i++)
{
B(i, 0) *= 10;
B(i, 1) *= 10;
}
}
}
if (goodFit)
{
// Remove the uncorrected coefficients
coeffsi[0] += deltaCoeff;
coeffsi[1] -= deltaCoeff;
}
else
{
WarningInFunction
<< "Could not fit face " << facei
<< " Coefficients = " << coeffsi
<< ", reverting to uncorrected." << endl;
coeffsi = 0;
}
}
int main(void){
// P2
unsigned int width(300), height(300);
kn::ioEPS eps(width, height);
//Draw axis
eps.drawLine(0, height/2, width, height/2, 0.5, 0.5, 0.5, 1);
eps.drawLine(width/2, 0, width/2, height, 0.5, 0.5, 0.5, 1);
unsigned int nbPoints(100);
#if 1
Eigen::MatrixXd pointsP2(nbPoints,3);
srand(time(NULL));
for(unsigned int i=0; i < nbPoints; ++i){
double x = (double(width - 100)/(nbPoints-1.0) * i) - double(width)/2.0 + 50.0;
double naturalOffset(10.0);
double randomOffset = (rand()%50) - 25.0;
pointsP2(i, 0) = x;
pointsP2(i, 1) = 0.8*x + naturalOffset + randomOffset;
pointsP2(i, 2) = 1.0;
}
//std::cout << " input data\n" << pointsP2 << std::endl << std::endl;
Eigen::Matrix3d signP2 = Eigen::Matrix3d::Identity();
signP2(1, 1) = -1.0;
//std::cout << " signP2\n" << signP2 << std::endl << std::endl;
Eigen::MatrixXd tmp(pointsP2);
tmp = pointsP2 * signP2;
std::cout << " input data\n" << tmp << std::endl << std::endl;
//SVD
Eigen::JacobiSVD<Eigen::MatrixXd> svd(tmp, Eigen::ComputeFullU | Eigen::ComputeFullV);
Eigen::VectorXd h = svd.matrixV().rightCols(1);
std::cout << " Solution\n" << h/h(1) << std::endl;
double graduate(h(0)/h(1));
double offset = h(2)/h(1);// (h(2) / sqrt(h(0)*h(0) + h(1) * h(1)));
//double offset((h(2)/h(1)) / sqrt(h(0)*h(0) + h(1) * h(1)));
std::cout << "Pente: " << graduate << " Offset: " << offset << std::endl;
#else
Eigen::MatrixXd pointsP2(nbPoints,2);
Eigen::VectorXd yP2(nbPoints);
srand(time(NULL));
for(unsigned int i=0; i < nbPoints; ++i){
double x = (double(width - 100)/(nbPoints-1.0) * i) - double(width)/2.0 + 50.0;
double naturalOffset(10.0);
double randomOffset = (rand()%50) - 25.0;
pointsP2(i, 0) = x;
pointsP2(i, 1) = 1.0;
yP2(i) = 0.8*x + naturalOffset + randomOffset;
}
std::cout << " input data\n" << pointsP2 << std::endl << std::endl;
//SVD
Eigen::JacobiSVD<Eigen::MatrixXd> svd(pointsP2, Eigen::ComputeFullU | Eigen::ComputeFullV);
Eigen::VectorXd h = svd.solve(yP2);
std::cout << " Solution\n" << h << std::endl;
double graduate(h(0));
double offset = h(1);// (h(2) / sqrt(h(0)*h(0) + h(1) * h(1)));
//double offset((h(2)/h(1)) / sqrt(h(0)*h(0) + h(1) * h(1)));
std::cout << "Pente: " << graduate << " Offset: " << offset << std::endl;
#endif
// affichage des points et droites //////////////////////////////////////////////////////////////////
double xTop = ( ( double(height) / 2.0f) - offset ) / graduate;
double xBot = ( (-double(height) / 2.0f) - offset ) / graduate;
double yLeft = graduate * (- double(width)/2.0f ) + offset;
double yRight = graduate * ( double(width)/2.0f ) + offset;
bool alreadyIntersect(false);
double linePoint1x, linePoint1y;
double linePoint2x, linePoint2y;
if( (xTop >= (- double(width)/2.0f)) && (xTop <= double(width)/2.0f)){
std::cout << "Top" << std::endl;
linePoint1x = xTop;
linePoint1y = double(height)/2.0f;
alreadyIntersect = true;
}
if( (xBot >= (- double(width)/2.0f)) && (xBot <= double(width)/2.0f) ){
std::cout << "Bot" << std::endl;
if(alreadyIntersect){
linePoint2x = xBot;
linePoint2y = -double(height)/2.0f;
}
else{
linePoint1x = xBot;
linePoint1y = -double(height)/2.0f;
}
alreadyIntersect = true;
}
if( (yLeft > (- double(height)/2.0f)) && (yLeft < double(height)/2.0f) ){
std::cout << "Left" << std::endl;
if(alreadyIntersect){
linePoint2x = -double(width)/2.0f;
linePoint2y = yLeft;
}
else{
linePoint1x = -double(width)/2.0f;
linePoint1y = yLeft;
}
alreadyIntersect = true;
}
if( (yRight > (- double(height)/2.0f)) && (yRight < double(height)/2.0f) ){
std::cout << "Right" << std::endl;
if(alreadyIntersect){
linePoint2x = double(width)/2.0f;
linePoint2y = yRight;
}
else{
linePoint1x = double(width)/2.0f;
linePoint1y = yRight;
}
alreadyIntersect = true;
}
/*
std::cout << linePoint1x << " " << linePoint1y << std::endl;
std::cout << linePoint2x << " " << linePoint2y << std::endl;
std::cout << linePoint1x + width / 2.0 << " " << height - (linePoint1y + height / 2.0) << std::endl;
std::cout << linePoint2x + width / 2.0 << " " << height - (linePoint2y + height / 2.0) << std::endl;
*/
eps.drawLine(linePoint1x + width / 2.0, height - (linePoint1y + height / 2.0),
linePoint2x + width / 2.0, height - (linePoint2y + height / 2.0),
0.0, 0.0, 1.0,
1);
// draw points
for( unsigned int i=0; i<pointsP2.rows(); ++i ){
eps.drawCircleFilled(width/2 + pointsP2(i, 0), height/2 - pointsP2(i, 1), 1, 1.0, 0.0, 0.0);
//eps.drawCircleFilled(width/2 + pointsP2(i, 0), height/2 - yP2(i), 1, 1.0, 0.0, 0.0);
}
eps.saveEPS("out.eps");
return 0;
}
slep_result_t malsar_low_rank(
CDotFeatures* features,
double* y,
double rho,
const slep_options& options)
{
int task;
int n_feats = features->get_dim_feature_space();
SG_SDEBUG("n feats = %d\n", n_feats);
int n_vecs = features->get_num_vectors();
SG_SDEBUG("n vecs = %d\n", n_vecs);
int n_tasks = options.n_tasks;
SG_SDEBUG("n tasks = %d\n", n_tasks);
int iter = 0;
// initialize weight vector and bias for each task
MatrixXd Ws = MatrixXd::Zero(n_feats, n_tasks);
VectorXd Cs = VectorXd::Zero(n_tasks);
for (task=0; task<n_tasks; task++)
{
int n_pos = 0;
int n_neg = 0;
for (int i=options.ind[task]; i<options.ind[task+1]; i++)
{
if (y[i] > 0)
n_pos++;
else
n_neg++;
}
Cs[task] = CMath::log(double(n_pos)/n_neg);
}
MatrixXd Wz=Ws, Wzp=Ws, Wz_old=Ws, delta_Wzp=Ws, gWs=Ws;
VectorXd Cz=Cs, Czp=Cs, Cz_old=Cs, delta_Czp=Cs, gCs=Cs;
double t=1, t_old=0;
double gamma=1, gamma_inc=2;
double obj=0.0, obj_old=0.0;
internal::set_is_malloc_allowed(false);
bool done = false;
while (!done && iter <= options.max_iter)
{
double alpha = double(t_old - 1)/t;
// compute search point
Ws = (1+alpha)*Wz - alpha*Wz_old;
Cs = (1+alpha)*Cz - alpha*Cz_old;
// zero gradient
gWs.setZero();
gCs.setZero();
// compute gradient and objective at search point
double Fs = 0;
for (task=0; task<n_tasks; task++)
{
for (int i=options.ind[task]; i<options.ind[task+1]; i++)
{
double aa = -y[i]*(features->dense_dot(i, Ws.col(task).data(), n_feats)+Cs[task]);
double bb = CMath::max(aa,0.0);
// avoid underflow when computing exponential loss
Fs += (CMath::log(CMath::exp(-bb) + CMath::exp(aa-bb)) + bb)/n_vecs;
double b = -y[i]*(1 - 1/(1+CMath::exp(aa)))/n_vecs;
gCs[task] += b;
features->add_to_dense_vec(b, i, gWs.col(task).data(), n_feats);
}
}
gWs.noalias() += 2*rho*Ws;
// add regularizer
Fs += rho*Ws.squaredNorm();
double Fzp = 0.0;
// line search, Armijo-Goldstein scheme
while (true)
{
// compute trace projection of Ws - gWs/gamma with 2*rho/gamma
internal::set_is_malloc_allowed(true);
Wzp.setZero();
JacobiSVD<MatrixXd> svd(Ws - gWs/gamma,ComputeThinU | ComputeThinV);
for (int i=0; i<svd.singularValues().size(); i++)
{
if (svd.singularValues()[i] > 2*rho/gamma)
Wzp += svd.matrixU().col(i)*
svd.singularValues()[i]*
svd.matrixV().col(i).transpose();
}
internal::set_is_malloc_allowed(false);
// walk in direction of antigradient
Czp = Cs - gCs/gamma;
// compute objective at line search point
Fzp = 0.0;
for (task=0; task<n_tasks; task++)
{
for (int i=options.ind[task]; i<options.ind[task+1]; i++)
{
double aa = -y[i]*(features->dense_dot(i, Wzp.col(task).data(), n_feats)+Cs[task]);
double bb = CMath::max(aa,0.0);
Fzp += (CMath::log(CMath::exp(-bb) + CMath::exp(aa-bb)) + bb)/n_vecs;
}
}
Fzp += rho*Wzp.squaredNorm();
// compute delta between line search point and search point
delta_Wzp = Wzp - Ws;
delta_Czp = Czp - Cs;
// norms of delta
double nrm_delta_Wzp = delta_Wzp.squaredNorm();
double nrm_delta_Czp = delta_Czp.squaredNorm();
double r_sum = (nrm_delta_Wzp + nrm_delta_Czp)/2;
double Fzp_gamma = Fs + (delta_Wzp.transpose()*gWs).trace() +
(delta_Czp.transpose()*gCs).trace() +
(gamma/2)*nrm_delta_Wzp +
(gamma/2)*nrm_delta_Czp;
// break if delta is getting too small
if (r_sum <= 1e-20)
{
done = true;
break;
}
// break if objective at line searc point is smaller than Fzp_gamma
if (Fzp <= Fzp_gamma)
break;
else
gamma *= gamma_inc;
}
Wz_old = Wz;
Cz_old = Cz;
Wz = Wzp;
Cz = Czp;
// compute objective value
obj_old = obj;
obj = Fzp;
internal::set_is_malloc_allowed(true);
JacobiSVD<MatrixXd> svd(Wzp, EigenvaluesOnly);
obj += rho*svd.singularValues().sum();
internal::set_is_malloc_allowed(false);
// check if process should be terminated
switch (options.termination)
{
case 0:
if (iter>=2)
{
if ( CMath::abs(obj-obj_old) <= options.tolerance )
done = true;
}
break;
case 1:
if (iter>=2)
{
if ( CMath::abs(obj-obj_old) <= options.tolerance*CMath::abs(obj_old))
done = true;
}
break;
case 2:
if (CMath::abs(obj) <= options.tolerance)
done = true;
break;
case 3:
if (iter>=options.max_iter)
done = true;
break;
}
iter++;
t_old = t;
t = 0.5 * (1 + CMath::sqrt(1.0 + 4*t*t));
}
SG_SDEBUG("%d iteration passed, objective = %f\n",iter,obj);
SGMatrix<float64_t> tasks_w(n_feats, n_tasks);
for (int i=0; i<n_feats; i++)
{
for (task=0; task<n_tasks; task++)
tasks_w[i] = Wzp(i,task);
}
SGVector<float64_t> tasks_c(n_tasks);
for (int i=0; i<n_tasks; i++) tasks_c[i] = Czp[i];
return slep_result_t(tasks_w, tasks_c);
};
/*
* Vertex positioning is based on [Kobbelt et al, 2001]
*/
float Octree::findVertex(Evaluator* e)
{
findIntersections(e);
// Find the center of intersection positions
glm::vec3 center = std::accumulate(
intersections.begin(), intersections.end(), glm::vec3(),
[](const glm::vec3& a, const Intersection& b)
{ return a + b.pos; }) / float(intersections.size());
/* The A matrix is of the form
* [n1x, n1y, n1z]
* [n2x, n2y, n2z]
* [n3x, n3y, n3z]
* ...
* (with one row for each Hermite intersection)
*/
Eigen::MatrixX3f A(intersections.size(), 3);
for (unsigned i=0; i < intersections.size(); ++i)
{
auto d = intersections[i].norm;
A.row(i) << Eigen::Vector3f(d.x, d.y, d.z).transpose();
}
/* The B matrix is of the form
* [p1 . n1]
* [p2 . n2]
* [p3 . n3]
* ...
* (with one row for each Hermite intersection)
*
* Positions are pre-emtively shifted so that the center of the contoru
* is at 0, 0, 0 (since the least-squares fix minimizes distance to the
* origin); we'll unshift afterwards.
*/
Eigen::VectorXf B(intersections.size(), 1);
for (unsigned i=0; i < intersections.size(); ++i)
{
B.row(i) << glm::dot(intersections[i].norm,
intersections[i].pos - center);
}
// Use singular value decomposition to solve the least-squares fit.
Eigen::JacobiSVD<Eigen::MatrixX3f> svd(A, Eigen::ComputeFullU |
Eigen::ComputeFullV);
// Truncate singular values below 0.1
auto singular = svd.singularValues();
svd.setThreshold(0.1 / singular.maxCoeff());
rank = svd.rank();
// Solve the equation and convert back to cell coordinates
Eigen::Vector3f solution = svd.solve(B);
vert = glm::vec3(solution.x(), solution.y(), solution.z()) + center;
// Clamp vertex to be within the bounding box
vert.x = std::min(X.upper(), std::max(vert.x, X.lower()));
vert.y = std::min(Y.upper(), std::max(vert.y, Y.lower()));
vert.z = std::min(Z.upper(), std::max(vert.z, Z.lower()));
// Find and return QEF residual
auto m = A * solution - B;
return m.transpose() * m;
}
at::Mat homographyToPose(at::real fx, at::real fy,
at::real tagSize,
const at::Mat& horig,
bool openGLStyle) {
// flip the homography along the Y axis to align the
// conventional image coordinate system (y=0 at the top) with
// the conventional camera coordinate system (y=0 at the
// bottom).
at::Mat h = horig;
at::Mat F = at::Mat::eye(3,3);
F(1,1) = F(2,2) = -1;
h = F * horig;
at::Mat M(4,4);
M(0,0) = h(0,0) / fx;
M(0,1) = h(0,1) / fx;
M(0,3) = h(0,2) / fx;
M(1,0) = h(1,0) / fy;
M(1,1) = h(1,1) / fy;
M(1,3) = h(1,2) / fy;
M(2,0) = h(2,0);
M(2,1) = h(2,1);
M(2,3) = h(2,2);
// Compute the scale. The columns of M should be made to be
// unit vectors. This is over-determined, so we take the
// geometric average.
at::real scale0 = sqrt(sq(M(0,0)) + sq(M(1,0)) + sq(M(2,0)));
at::real scale1 = sqrt(sq(M(0,1)) + sq(M(1,1)) + sq(M(2,1)));
at::real scale = sqrt(scale0*scale1);
M *= 1.0/scale;
// recover sign of scale factor by noting that observations must
// occur in front of the camera.
if (M(2,3) > 0) {
M *= -1;
}
// The bottom row should always be [0 0 0 1].
M(3,0) = 0;
M(3,1) = 0;
M(3,2) = 0;
M(3,3) = 1;
// recover third rotation vector by crossproduct of the other two
// rotation vectors
at::Vec3 a( M(0,0), M(1,0), M(2,0) );
at::Vec3 b( M(0,1), M(1,1), M(2,1) );
at::Vec3 ab = a.cross(b);
M(0,2) = ab[0];
M(1,2) = ab[1];
M(2,2) = ab[2];
// pull out just the rotation component so we can normalize it.
at::Mat R(3,3);
for (int i=0; i<3; ++i) {
for (int j=0; j<3; ++j) {
R(i,j) = M(i,j);
}
}
// polar decomposition, R = (UV')(VSV')
cv::SVD svd(R);
at::Mat MR = svd.u * svd.vt;
if (!openGLStyle) { MR = F * MR; }
for (int i=0; i<3; ++i) {
for (int j=0; j<3; ++j) {
M(i,j) = MR(i,j);
}
}
// Scale the results based on the scale in the homography. The
// homography assumes that tags span from -1 to +1, i.e., that
// they are two units wide (and tall).
for (int i = 0; i < 3; i++) {
at::real scl = openGLStyle ? 1 : F(i,i);
M(i,3) *= scl * tagSize / 2;
}
return M;
}
// ****************************************************************************
// This function finds a set of vectors (P) such that, for each vector p:
// A.p = 0
// Note that A is a matrix of size rows x cols, P is a matrix of size cols x cols whose ROWS are the p vectors.
// P must NOT point to any allocated memory prior to calling this function.
// The rank deficit (nullity) is returned (0 if matrix is full rank, 1 if matrix has one singular value, etc).
// tol defines how small a singular value must be in order to be considered a "zero".
// ****************************************************************************
long int find_null_space_vectors(double *A, long int rows, long int cols, double **P, double tolerance)
{
double *U, *D, *V, *R, max_val, min_val;
long int i, rank_deficit, c, r;
U=(double *)malloc(sizeof(double)*rows*cols);
if (U==NULL) quit_error((char*)"Out of memory");
D=(double *)malloc(sizeof(double)*cols*cols);
if (D==NULL) quit_error((char*)"Out of memory");
V=(double *)malloc(sizeof(double)*cols*cols);
if (V==NULL) quit_error((char*)"Out of memory");
// Calculate the Singular Value Decomposition
svd(A, rows, cols, U, D, V);
// Find the maximum and minimum magnitude singular values
max_val=0.0;
min_val=DBL_MAX;
for (i=0; i<cols; i++)
{
if (fabs(D[i*cols+i])<fabs(min_val)) min_val=fabs(D[i*cols+i]);
if (fabs(D[i*cols+i])>fabs(max_val)) max_val=fabs(D[i*cols+i]);
}
// Count the rank deficit of the matrix, and extract
// relevant rows of the transpose(V) matrix.
rank_deficit=0;
for (i=0; i<cols; i++)
{
if (fabs(D[i*cols+i])<tolerance) rank_deficit++;
}
// Transpose V
matrix_transpose(V, cols, cols);
// Generate a reduced version of transpose(V)
R=(double *)malloc(sizeof(double)*cols*(rank_deficit));
if (R==NULL) quit_error((char*)"Out of memory");
r=0;
for (i=0; i<cols; i++)
{
if (fabs(D[i*cols+i])<fabs(tolerance))
{
for (c=0; c<cols; c++)
{
R[r*cols+c]=V[i*cols+c];
}
r++;
}
}
// Row reduce R
matrix_row_reduce(R, rank_deficit, cols, tolerance);
*P=R;
free(V);
free(D);
free(U);
return rank_deficit;
}
void
v4r::MultiPlaneSegmentation<PointT>::segment(bool force_unorganized)
{
models_.clear();
if(input_->isOrganized() && !force_unorganized)
{
pcl::PointCloud<pcl::Normal>::Ptr normal_cloud (new pcl::PointCloud<pcl::Normal>);
if(!normals_set_)
{
pcl::IntegralImageNormalEstimation<PointT, pcl::Normal> ne;
ne.setNormalEstimationMethod (ne.COVARIANCE_MATRIX);
ne.setMaxDepthChangeFactor (0.02f);
ne.setNormalSmoothingSize (20.0f);
ne.setBorderPolicy (pcl::IntegralImageNormalEstimation<PointT, pcl::Normal>::BORDER_POLICY_IGNORE);
ne.setInputCloud (input_);
ne.compute (*normal_cloud);
}
else
{
normal_cloud = normal_cloud_;
}
pcl::OrganizedMultiPlaneSegmentation<PointT, pcl::Normal, pcl::Label> mps;
mps.setMinInliers (min_plane_inliers_);
mps.setAngularThreshold (0.017453 * 2.f); // 2 degrees
mps.setDistanceThreshold (0.01); // 1cm
mps.setMaximumCurvature(0.002);
mps.setInputNormals (normal_cloud);
mps.setInputCloud (input_);
std::vector<pcl::PlanarRegion<PointT>, Eigen::aligned_allocator<pcl::PlanarRegion<PointT> > > regions;
std::vector<pcl::ModelCoefficients> model_coefficients;
std::vector<pcl::PointIndices> inlier_indices;
pcl::PointCloud<pcl::Label>::Ptr labels (new pcl::PointCloud<pcl::Label>);
std::vector<pcl::PointIndices> label_indices;
std::vector<pcl::PointIndices> boundary_indices;
typename pcl::PlaneRefinementComparator<PointT, pcl::Normal, pcl::Label>::Ptr ref_comp (
new pcl::PlaneRefinementComparator<PointT,
pcl::Normal, pcl::Label> ());
ref_comp->setDistanceThreshold (0.01f, false);
ref_comp->setAngularThreshold (0.017453 * 2.f);
mps.setRefinementComparator (ref_comp);
mps.segmentAndRefine (regions, model_coefficients, inlier_indices, labels, label_indices, boundary_indices);
//mps.segment (model_coefficients, inlier_indices);
//std::cout << model_coefficients.size() << std::endl;
if(merge_planes_)
{
//sort planes by size
//check if the first plane can be merged against the others, if yes, define a new plane combining both and add it to the cue
typedef boost::adjacency_matrix<boost::undirectedS, int> GraphPlane;
GraphPlane mergeable_planes (model_coefficients.size ());
for(size_t i=0; i < model_coefficients.size(); i++)
{
Eigen::Vector3f plane_i = Eigen::Vector3f (model_coefficients[i].values[0], model_coefficients[i].values[1],
model_coefficients[i].values[2]);
plane_i.normalize();
for(size_t j=(i+1); j < model_coefficients.size(); j++)
{
Eigen::Vector3f plane_j = Eigen::Vector3f (model_coefficients[j].values[0], model_coefficients[j].values[1],
model_coefficients[j].values[2]);
plane_j.normalize();
//std::cout << "dot product:" << plane_i.dot(plane_j) << " diff:" << std::abs(model_coefficients[i].values[3] - model_coefficients[j].values[3]) << std::endl;
if(plane_i.dot(plane_j) > 0.95)
{
if(std::abs(model_coefficients[i].values[3] - model_coefficients[j].values[3]) < 0.015)
{
boost::add_edge (static_cast<int>(i), static_cast<int>(j), mergeable_planes);
}
}
}
}
boost::vector_property_map<int> components (boost::num_vertices (mergeable_planes));
int n_cc = static_cast<int> (boost::connected_components (mergeable_planes, &components[0]));
std::vector<int> cc_sizes;
std::vector< std::vector<int> > cc_to_model_coeff;
cc_sizes.resize (n_cc, 0);
cc_to_model_coeff.resize(n_cc);
for (size_t i = 0; i < model_coefficients.size (); i++)
{
cc_sizes[components[i]]++;
cc_to_model_coeff[components[i]].push_back(i);
}
std::vector<pcl::ModelCoefficients> new_model_coefficients;
std::vector<pcl::PointIndices> new_inlier_indices;
for(size_t i=0; i < cc_sizes.size(); i++)
{
if(cc_sizes[i] < 2)
{
new_model_coefficients.push_back(model_coefficients[cc_to_model_coeff[i][0]]);
new_inlier_indices.push_back(inlier_indices[cc_to_model_coeff[i][0]]);
continue;
}
//std::cout << "going to merge CC:" << cc_sizes[i] << std::endl;
pcl::ModelCoefficients model_coeff;
model_coeff.values.resize(4);
for(size_t k=0; k < 4; k++)
model_coeff.values[k] = 0.f;
pcl::PointIndices merged_indices;
for(size_t j=0; j < cc_to_model_coeff[i].size(); j++)
{
for(size_t k=0; k < 4; k++)
model_coeff.values[k] += model_coefficients[cc_to_model_coeff[i][j]].values[k];
merged_indices.indices.insert(merged_indices.indices.end(), inlier_indices[cc_to_model_coeff[i][j]].indices.begin(),
inlier_indices[cc_to_model_coeff[i][j]].indices.end());
}
for(size_t k=0; k < 4; k++)
model_coeff.values[k] /= static_cast<float>(cc_to_model_coeff[i].size());
new_model_coefficients.push_back(model_coeff);
new_inlier_indices.push_back(merged_indices);
}
model_coefficients = new_model_coefficients;
inlier_indices = new_inlier_indices;
}
for(size_t i=0; i < model_coefficients.size(); i++)
{
PlaneModel<PointT> pm;
pm.coefficients_ = model_coefficients[i];
pm.cloud_ = input_;
pm.inliers_ = inlier_indices[i];
//recompute coefficients based on distance to camera and normal?
Eigen::Vector4f centroid;
pcl::compute3DCentroid(*pm.cloud_, pm.inliers_, centroid);
Eigen::Vector3f c(centroid[0],centroid[1],centroid[2]);
Eigen::MatrixXf M_w(inlier_indices[i].indices.size(), 3);
float sum_w = 0.f;
for(size_t k=0; k < inlier_indices[i].indices.size(); k++)
{
const int &idx = inlier_indices[i].indices[k];
float d_c = (pm.cloud_->points[idx].getVector3fMap()).norm();
float w_k = std::max(1.f - std::abs(1.f - d_c), 0.f);
//w_k = 1.f;
M_w.row(k) = w_k * (pm.cloud_->points[idx].getVector3fMap() - c);
sum_w += w_k;
}
Eigen::Matrix3f scatter;
scatter.setZero ();
scatter = M_w.transpose() * M_w;
Eigen::JacobiSVD<Eigen::MatrixXf> svd(scatter, Eigen::ComputeFullV);
//std::cout << svd.matrixV() << std::endl;
Eigen::Vector3f n = svd.matrixV().col(2);
//flip normal if required
if(n.dot(c*-1) < 0)
n = n * -1.f;
float d = n.dot(c) * -1.f;
//std::cout << "normal:" << n << std::endl;
//std::cout << "d:" << d << std::endl;
pm.coefficients_.values[0] = n[0];
pm.coefficients_.values[1] = n[1];
pm.coefficients_.values[2] = n[2];
pm.coefficients_.values[3] = d;
pcl::PointIndices clean_inlier_indices;
float dist_threshold_ = 0.01f;
for(size_t k=0; k < inlier_indices[i].indices.size(); k++)
{
const int &idx = inlier_indices[i].indices[k];
Eigen::Vector3f p = pm.cloud_->points[idx].getVector3fMap();
float val = n.dot(p) + d;
if(std::abs(val) <= dist_threshold_)
clean_inlier_indices.indices.push_back( idx );
}
pm.inliers_ = clean_inlier_indices;
models_.push_back(pm);
}
}
else
{
// Create the filtering object: downsample the dataset using a leaf size of 1cm
pcl::VoxelGrid<PointT> vg;
PointTCloudPtr cloud_filtered (new PointTCloud);
vg.setInputCloud (input_);
float leaf_size_ = 0.005f;
float dist_threshold_ = 0.01f;
vg.setLeafSize (leaf_size_, leaf_size_, leaf_size_);
vg.filter (*cloud_filtered);
std::vector<bool> pixel_has_not_been_labelled( cloud_filtered->points.size(), true );
// Create the segmentation object for the planar model and set all the parameters
pcl::SACSegmentation<PointT> seg;
pcl::ModelCoefficients coefficients;
seg.setOptimizeCoefficients (true);
seg.setModelType (pcl::SACMODEL_PLANE);
seg.setMethodType (pcl::SAC_RANSAC);
seg.setMaxIterations (1000);
seg.setDistanceThreshold (dist_threshold_);
PointTCloudPtr cloud_filtered_leftover (new PointTCloud (*cloud_filtered));
while (1)
{
// Segment the largest planar component from the remaining cloud
seg.setInputCloud (cloud_filtered_leftover);
pcl::PointIndices inliers_in_leftover;
seg.segment (inliers_in_leftover, coefficients);
std::cout << "inliers in left over: " << inliers_in_leftover.indices.size() << " of " << cloud_filtered_leftover->points.size() << std::endl;
if ( (int)inliers_in_leftover.indices.size () < min_plane_inliers_) // Could not estimate a(nother) planar model big enough for the given cloud.
break;
// make indices correspond to complete downsampled cloud
pcl::PointIndices indices_in_original_cloud;
size_t current_index_in_leftover = 0;
size_t px=0;
indices_in_original_cloud.indices.resize(inliers_in_leftover.indices.size());
for(size_t inl_id=0; inl_id < inliers_in_leftover.indices.size(); inl_id++) { // assumes indices are sorted in ascending order
bool found = false;
do {
if( pixel_has_not_been_labelled[px] ) {
if( current_index_in_leftover == inliers_in_leftover.indices [inl_id] ) {
indices_in_original_cloud.indices[ inl_id ] = px;
found = true;
}
current_index_in_leftover++;
}
px++;
} while( !found );
}
for(size_t i=0; i<indices_in_original_cloud.indices.size(); i++)
pixel_has_not_been_labelled[ indices_in_original_cloud.indices[i] ] = false;
//save coefficients
PlaneModel<PointT> pm;
pm.coefficients_ = coefficients;
pm.cloud_ = cloud_filtered;
pm.inliers_ = indices_in_original_cloud;
models_.push_back(pm);
pcl::copyPointCloud(*cloud_filtered, pixel_has_not_been_labelled, *cloud_filtered_leftover);
}
std::cout << "Number of planes found:" << models_.size() << "organized:" << static_cast<int>(input_->isOrganized() && !force_unorganized) << std::endl;
}
}
void EM::mStep()
{
// Update means_k, covs_k and weights_k from probs_ik
int dim = trainSamples.cols;
// Update weights
// not normalized first
reduce(trainProbs, weights, 0, CV_REDUCE_SUM);
// Update means
means.create(nclusters, dim, CV_64FC1);
means = Scalar(0);
const double minPosWeight = trainSamples.rows * DBL_EPSILON;
double minWeight = DBL_MAX;
int minWeightClusterIndex = -1;
for(int clusterIndex = 0; clusterIndex < nclusters; clusterIndex++)
{
if(weights.at<double>(clusterIndex) <= minPosWeight)
continue;
if(weights.at<double>(clusterIndex) < minWeight)
{
minWeight = weights.at<double>(clusterIndex);
minWeightClusterIndex = clusterIndex;
}
Mat clusterMean = means.row(clusterIndex);
for(int sampleIndex = 0; sampleIndex < trainSamples.rows; sampleIndex++)
clusterMean += trainProbs.at<double>(sampleIndex, clusterIndex) * trainSamples.row(sampleIndex);
clusterMean /= weights.at<double>(clusterIndex);
}
// Update covsEigenValues and invCovsEigenValues
covs.resize(nclusters);
covsEigenValues.resize(nclusters);
if(covMatType == EM::COV_MAT_GENERIC)
covsRotateMats.resize(nclusters);
invCovsEigenValues.resize(nclusters);
for(int clusterIndex = 0; clusterIndex < nclusters; clusterIndex++)
{
if(weights.at<double>(clusterIndex) <= minPosWeight)
continue;
if(covMatType != EM::COV_MAT_SPHERICAL)
covsEigenValues[clusterIndex].create(1, dim, CV_64FC1);
else
covsEigenValues[clusterIndex].create(1, 1, CV_64FC1);
if(covMatType == EM::COV_MAT_GENERIC)
covs[clusterIndex].create(dim, dim, CV_64FC1);
Mat clusterCov = covMatType != EM::COV_MAT_GENERIC ?
covsEigenValues[clusterIndex] : covs[clusterIndex];
clusterCov = Scalar(0);
Mat centeredSample;
for(int sampleIndex = 0; sampleIndex < trainSamples.rows; sampleIndex++)
{
centeredSample = trainSamples.row(sampleIndex) - means.row(clusterIndex);
if(covMatType == EM::COV_MAT_GENERIC)
clusterCov += trainProbs.at<double>(sampleIndex, clusterIndex) * centeredSample.t() * centeredSample;
else
{
double p = trainProbs.at<double>(sampleIndex, clusterIndex);
for(int di = 0; di < dim; di++ )
{
double val = centeredSample.at<double>(di);
clusterCov.at<double>(covMatType != EM::COV_MAT_SPHERICAL ? di : 0) += p*val*val;
}
}
}
if(covMatType == EM::COV_MAT_SPHERICAL)
clusterCov /= dim;
clusterCov /= weights.at<double>(clusterIndex);
// Update covsRotateMats for EM::COV_MAT_GENERIC only
if(covMatType == EM::COV_MAT_GENERIC)
{
SVD svd(covs[clusterIndex], SVD::MODIFY_A + SVD::FULL_UV);
covsEigenValues[clusterIndex] = svd.w;
covsRotateMats[clusterIndex] = svd.u;
}
max(covsEigenValues[clusterIndex], minEigenValue, covsEigenValues[clusterIndex]);
// update invCovsEigenValues
invCovsEigenValues[clusterIndex] = 1./covsEigenValues[clusterIndex];
}
for(int clusterIndex = 0; clusterIndex < nclusters; clusterIndex++)
{
if(weights.at<double>(clusterIndex) <= minPosWeight)
{
Mat clusterMean = means.row(clusterIndex);
means.row(minWeightClusterIndex).copyTo(clusterMean);
covs[minWeightClusterIndex].copyTo(covs[clusterIndex]);
covsEigenValues[minWeightClusterIndex].copyTo(covsEigenValues[clusterIndex]);
if(covMatType == EM::COV_MAT_GENERIC)
covsRotateMats[minWeightClusterIndex].copyTo(covsRotateMats[clusterIndex]);
invCovsEigenValues[minWeightClusterIndex].copyTo(invCovsEigenValues[clusterIndex]);
}
}
// Normalize weights
weights /= trainSamples.rows;
}
bool VersionManager::isApkNeedUpdate()
{
return svd().ver.is_v2_high_to(lvd().ver);
}
