Eigen::Matrix3d make_inv_matr(
SuperCell& SCell
){
Eigen::Matrix3d invmat;
invmat.row(0) = ( SCell.T.row(1) ).cross( (SCell.T.row(2) )) / ( SCell.T.row( 0 ).dot( ( SCell.T.row(1) ).cross( (SCell.T.row(2) ))) );
invmat.row(1) = ( SCell.T.row(2) ).cross( (SCell.T.row(0) )) / ( SCell.T.row( 0 ).dot( ( SCell.T.row(1) ).cross( (SCell.T.row(2) ))) );
invmat.row(2) = ( SCell.T.row(0) ).cross( (SCell.T.row(1) )) / ( SCell.T.row( 0 ).dot( ( SCell.T.row(1) ).cross( (SCell.T.row(2) ))) );
return invmat;
}
void CETranslateWidget::checkSelection()
{
// User closed the dialog:
if (this->isHidden()) {
m_selectionTimer.stop();
return;
}
// Improper initialization
if (m_gl == NULL)
return setError(tr("No GLWidget?"));
QList<Primitive*> atoms = m_gl->selectedPrimitives().subList
(Primitive::AtomType);
// No selection
if (!atoms.size())
return setError(tr("Please select one or more atoms."));
clearError();
// Calculate centroid of selection
m_vector = Eigen::Vector3d::Zero();
for (QList<Primitive*>::const_iterator
it = atoms.constBegin(),
it_end = atoms.constEnd();
it != it_end; ++it) {
m_vector += (*qobject_cast<Atom* const>(*it)->pos());
}
m_vector /= static_cast<double>(atoms.size());
switch (static_cast<TranslateMode>
(ui.combo_translateMode->currentIndex())) {
default:
case Avogadro::CETranslateWidget::TM_VECTOR:
// Shouldn't happen, but just in case...
m_selectionTimer.stop();
this->enableVectorEditor();
break;
case Avogadro::CETranslateWidget::TM_ATOM_TO_ORIGIN:
m_vector = -m_vector;
break;
case Avogadro::CETranslateWidget::TM_ATOM_TO_CELL_CENTER:
// Calculate center of unit cell
const Eigen::Matrix3d cellRowMatrix = m_ext->currentCellMatrix();
const Eigen::Vector3d center = 0.5 *
(cellRowMatrix.row(0) + cellRowMatrix.row(1) + cellRowMatrix.row(2));
// Calculate necessary translation
m_vector = center - m_vector;
break;
}
updateGui();
}
void Reconstruction3D::RQdecomposition(Eigen::MatrixXd A, Eigen::Matrix3d &R, Eigen::Matrix3d &Q)
{
float c, s;
Eigen::Matrix3d Qx, Qy, Qz;
Eigen::Matrix3d M;
M << A(0, 0), A(0, 1), A(0, 2),
A(1, 0), A(1, 1), A(1, 2),
A(2, 0), A(2, 1), A(2, 2);
R = M;
Reconstruction3D::solveCS(c, s, R(2, 1), R(2, 2));
Qx << 1, 0, 0,
0, c, -s,
0, s, c;
R *= Qx;
Reconstruction3D::solveCS(c, s, R(2, 0), -R(2, 2));
Qy << c, 0, s,
0, 1, 0,
-s, 0, c;
R *= Qy;
Reconstruction3D::solveCS(c, s, R(1, 0), R(1, 1));
Qz << c, -s, 0,
s, c, 0,
0, 0, 1;
R *= Qz;
if (std::abs(R(1, 0)) > 0.00005 || std::abs(R(2, 0)) > 0.00005 || std::abs(R(2, 1)) > 0.00005)
std::cerr << "PROBLEM WITH RQdecomposition" << std::endl;;
// if(R(1,0)!= 0) R(1,0) = 0;
// if(R(2,0)!= 0) R(2,0) = 0;
// if(R(2,1)!= 0) R(2,1) = 0;
Q = Qz.transpose() * Qy.transpose() * Qx.transpose();
if (R(0, 0) < 0)
{
R.col(0) *= -1;
Q.row(0) *= -1;
}
if (R(1, 1) < 0)
{
R.col(1) *= -1;
Q.row(1) *= -1;
}
if (R(2, 2) < 0)
{
R.col(2) *= -1;
Q.row(2) *= -1;
}
}
void BoundingBox::computeOrientedBox(std::vector<Eigen::Vector3d>& positions)
{
// compute mean
Eigen::Vector3d cm;
cm.setZero();
for (size_t i = 0; i < positions.size(); i++) {
cm += positions[i];
}
cm /= (double)positions.size();
// adjust for mean and compute covariance matrix
Eigen::Matrix3d covariance;
covariance.setZero();
for (size_t i = 0; i < positions.size(); i++) {
Eigen::Vector3d pAdg = positions[i] - cm;
covariance += pAdg * pAdg.transpose();
}
covariance /= (double)positions.size();
// compute eigenvectors for covariance matrix
Eigen::EigenSolver<Eigen::Matrix3d> solver(covariance);
Eigen::Matrix3d eigenVectors = solver.eigenvectors().real();
// set axes
eigenVectors.transpose();
xAxis = eigenVectors.row(0);
yAxis = eigenVectors.row(1);
zAxis = eigenVectors.row(2);
// project min and max points on each principal axis
double min1 = INF, max1 = -INF;
double min2 = INF, max2 = -INF;
double min3 = INF, max3 = -INF;
double d = 0.0;
for (size_t i = 0; i < positions.size(); i++) {
d = xAxis.dot(positions[i]);
if (min1 > d) min1 = d;
if (max1 < d) max1 = d;
d = yAxis.dot(positions[i]);
if (min2 > d) min2 = d;
if (max2 < d) max2 = d;
d = zAxis.dot(positions[i]);
if (min3 > d) min3 = d;
if (max3 < d) max3 = d;
}
// set center and halflengths
center = (xAxis*(min1 + max1) + yAxis*(min2 + max2) + zAxis*(min3 + max3)) /2;
halfLx = (max1 - min1)/2; halfLy = (max2 - min2)/2; halfLz = (max3 - min3)/2;
}
void CEViewOptionsWidget::updateMillerPlane()
{
// View into a Miller plane
Camera *camera = m_glWidget->camera();
Eigen::Transform3d modelView;
modelView.setIdentity();
// OK, so we want to rotate to look along the normal at the plane
// So we convert <h k l> into a Cartesian normal
Eigen::Matrix3d cellMatrix = m_ext->unconvertLength(m_ext->currentCellMatrix()).transpose();
// Get miller indices:
const Eigen::Vector3d millerIndices
(static_cast<double>(ui.spin_mi_h->value()),
static_cast<double>(ui.spin_mi_k->value()),
static_cast<double>(ui.spin_mi_l->value()));
// Check to see if we have 0,0,0
// in which case, we do nothing
if (millerIndices.squaredNorm() < 0.5)
return;
const Eigen::Vector3d normalVector ((cellMatrix * millerIndices).normalized());
Eigen::Matrix3d rotation;
rotation.row(2) = normalVector;
rotation.row(0) = rotation.row(2).unitOrthogonal();
rotation.row(1) = rotation.row(2).cross(rotation.row(0));
// Translate camera to the center of the cell
const Eigen::Vector3d cellDiagonal =
cellMatrix.col(0) * m_glWidget->aCells() +
cellMatrix.col(1) * m_glWidget->bCells() +
cellMatrix.col(2) * m_glWidget->cCells();
modelView.translate(-cellDiagonal*0.5);
// Prerotate the camera to look down the specified normal
modelView.prerotate(rotation);
// Pretranslate in the negative Z direction
modelView.pretranslate(Eigen::Vector3d(0.0, 0.0,
-1.5 * cellDiagonal.norm()));
camera->setModelview(modelView);
// Call for a redraw
m_glWidget->update();
}
void EKF::predict(Eigen::Matrix3d Hom)
{
// homogeneous coordinates
Eigen::Vector3d x_tilde;
x_tilde << this->x(0), this->x(1), 1.0;
// intermediary values
double gx = Hom.row(0) * x_tilde;
double gy = Hom.row(1) * x_tilde;
double h = Hom.row(2) * x_tilde;
// update transition matrix
Eigen::Matrix2d F;
F <<
(Hom(0, 0) * h - Hom(2, 0) * gx) / (h*h), (Hom(0, 1) * h - Hom(2, 1) * gx) / (h*h),
(Hom(1, 0) * h - Hom(2, 0) * gy) / (h*h), (Hom(1, 1) * h - Hom(2, 1) * gy) / (h*h);
// propogate
this->x = Eigen::Vector2d(gx / h, gy / h);
this->P = F * this->P * F.transpose() + this->Q;
}
void BoundingBox::computeOrientedBox(std::vector<Vertex>& vertices)
{
type = "Oriented";
orientedPoints.clear();
// compute mean
Eigen::Vector3d center;
center.setZero();
for (VertexCIter v = vertices.begin(); v != vertices.end(); v++) {
center += v->position;
}
center /= (double)vertices.size();
// adjust for mean and compute covariance
Eigen::Matrix3d covariance;
covariance.setZero();
for (VertexIter v = vertices.begin(); v != vertices.end(); v++) {
Eigen::Vector3d pAdg = v->position - center;
covariance += pAdg * pAdg.transpose();
}
covariance /= (double)vertices.size();
// compute eigenvectors for the covariance matrix
Eigen::EigenSolver<Eigen::Matrix3d> solver(covariance);
Eigen::Matrix3d eigenVectors = solver.eigenvectors().real();
// project min and max points on each principal axis
double min1 = INFINITY, max1 = -INFINITY;
double min2 = INFINITY, max2 = -INFINITY;
double min3 = INFINITY, max3 = -INFINITY;
double d = 0.0;
eigenVectors.transpose();
for (VertexIter v = vertices.begin(); v != vertices.end(); v++) {
d = eigenVectors.row(0).dot(v->position);
if (min1 > d) min1 = d;
if (max1 < d) max1 = d;
d = eigenVectors.row(1).dot(v->position);
if (min2 > d) min2 = d;
if (max2 < d) max2 = d;
d = eigenVectors.row(2).dot(v->position);
if (min3 > d) min3 = d;
if (max3 < d) max3 = d;
}
// add points to vector
orientedPoints.push_back(eigenVectors.row(0) * min1);
orientedPoints.push_back(eigenVectors.row(0) * max1);
orientedPoints.push_back(eigenVectors.row(1) * min2);
orientedPoints.push_back(eigenVectors.row(1) * max2);
orientedPoints.push_back(eigenVectors.row(2) * min3);
orientedPoints.push_back(eigenVectors.row(2) * max3);
}
void
stereo_rectify(const CameraIntrinsicsParameters& left_params,
const CameraIntrinsicsParameters& right_params,
const Eigen::Quaterniond& rotation_quat,
const Eigen::Vector3d& translation,
Eigen::Matrix3d* left_rotation,
Eigen::Matrix3d* right_rotation,
CameraIntrinsicsParameters* rectified_params)
{
// Get two cameras to same orientation, minimally rotating each.
Eigen::AngleAxisd rect_rot(rotation_quat);
rect_rot.angle() *= -0.5;
Eigen::Vector3d rect_trans = rect_rot * translation;
// Bring translation to alignment with (1, 0, 0).
Eigen::Matrix3d rot;
rot.row(0) = -rect_trans;
rot.row(0).normalize();
rot.row(1) = Eigen::Vector3d(0, 0, 1).cross(rot.row(0));
rot.row(2) = rot.row(0).cross(rot.row(1));
//std::cerr << "rot =\n" << rot << std::endl << std::endl;
(*left_rotation) = rot * rect_rot.inverse();
(*right_rotation) = rot * rect_rot;
// For now just use left camera's intrinsic parameters
*rectified_params = left_params;
// Just to be explicit
rectified_params->k1 = 0;
rectified_params->k2 = 0;
rectified_params->k3 = 0;
rectified_params->p1 = 0;
rectified_params->p2 = 0;
}
int P3P::computePoses(const Eigen::Matrix3d & feature_vectors, const Eigen::Matrix3d & world_points,
Eigen::Matrix<Eigen::Matrix<double, 3, 4>, 4, 1> & solutions)
{
// Extraction of world points
Eigen::Vector3d P1 = world_points.col(0);
Eigen::Vector3d P2 = world_points.col(1);
Eigen::Vector3d P3 = world_points.col(2);
// Verification that world points are not colinear
Eigen::Vector3d temp1 = P2 - P1;
Eigen::Vector3d temp2 = P3 - P1;
if (temp1.cross(temp2).norm() == 0)
{
return -1;
}
// Extraction of feature vectors
Eigen::Vector3d f1 = feature_vectors.col(0);
Eigen::Vector3d f2 = feature_vectors.col(1);
Eigen::Vector3d f3 = feature_vectors.col(2);
// Creation of intermediate camera frame
Eigen::Vector3d e1 = f1;
Eigen::Vector3d e3 = f1.cross(f2);
e3 = e3 / e3.norm();
Eigen::Vector3d e2 = e3.cross(e1);
Eigen::Matrix3d T;
T.row(0) = e1.transpose();
T.row(1) = e2.transpose();
T.row(2) = e3.transpose();
f3 = T * f3;
// Reinforce that f3(2,0) > 0 for having theta in [0;pi]
if (f3(2, 0) > 0)
{
f1 = feature_vectors.col(1);
f2 = feature_vectors.col(0);
f3 = feature_vectors.col(2);
e1 = f1;
e3 = f1.cross(f2);
e3 = e3 / e3.norm();
e2 = e3.cross(e1);
T.row(0) = e1.transpose();
T.row(1) = e2.transpose();
T.row(2) = e3.transpose();
f3 = T * f3;
P1 = world_points.col(1);
P2 = world_points.col(0);
P3 = world_points.col(2);
}
// Creation of intermediate world frame
Eigen::Vector3d n1 = P2 - P1;
n1 = n1 / n1.norm();
Eigen::Vector3d n3 = n1.cross(P3 - P1);
n3 = n3 / n3.norm();
Eigen::Vector3d n2 = n3.cross(n1);
Eigen::Matrix3d N;
N.row(0) = n1.transpose();
N.row(1) = n2.transpose();
N.row(2) = n3.transpose();
// Extraction of known parameters
P3 = N * (P3 - P1);
double d_12 = (P2 - P1).norm();
double f_1 = f3(0, 0) / f3(2, 0);
double f_2 = f3(1, 0) / f3(2, 0);
double p_1 = P3(0, 0);
double p_2 = P3(1, 0);
double cos_beta = f1.dot(f2);
double b = 1 / (1 - pow(cos_beta, 2)) - 1;
if (cos_beta < 0)
{
b = -sqrt(b);
}
else
{
b = sqrt(b);
}
// Definition of temporary variables for avoiding multiple computation
double f_1_pw2 = pow(f_1, 2);
double f_2_pw2 = pow(f_2, 2);
double p_1_pw2 = pow(p_1, 2);
double p_1_pw3 = p_1_pw2 * p_1;
double p_1_pw4 = p_1_pw3 * p_1;
double p_2_pw2 = pow(p_2, 2);
double p_2_pw3 = p_2_pw2 * p_2;
double p_2_pw4 = p_2_pw3 * p_2;
double d_12_pw2 = pow(d_12, 2);
double b_pw2 = pow(b, 2);
// Computation of factors of 4th degree polynomial
Eigen::Matrix<double, 5, 1> factors;
factors(0) = -f_2_pw2 * p_2_pw4 - p_2_pw4 * f_1_pw2 - p_2_pw4;
factors(1) = 2 * p_2_pw3 * d_12 * b + 2 * f_2_pw2 * p_2_pw3 * d_12 * b - 2 * f_2 * p_2_pw3 * f_1 * d_12;
factors(2) = -f_2_pw2 * p_2_pw2 * p_1_pw2 - f_2_pw2 * p_2_pw2 * d_12_pw2 * b_pw2 - f_2_pw2 * p_2_pw2 * d_12_pw2
+ f_2_pw2 * p_2_pw4 + p_2_pw4 * f_1_pw2 + 2 * p_1 * p_2_pw2 * d_12 + 2 * f_1 * f_2 * p_1 * p_2_pw2 * d_12 * b
- p_2_pw2 * p_1_pw2 * f_1_pw2 + 2 * p_1 * p_2_pw2 * f_2_pw2 * d_12 - p_2_pw2 * d_12_pw2 * b_pw2
- 2 * p_1_pw2 * p_2_pw2;
factors(3) = 2 * p_1_pw2 * p_2 * d_12 * b + 2 * f_2 * p_2_pw3 * f_1 * d_12 - 2 * f_2_pw2 * p_2_pw3 * d_12 * b
- 2 * p_1 * p_2 * d_12_pw2 * b;
factors(4) = -2 * f_2 * p_2_pw2 * f_1 * p_1 * d_12 * b + f_2_pw2 * p_2_pw2 * d_12_pw2 + 2 * p_1_pw3 * d_12
- p_1_pw2 * d_12_pw2 + f_2_pw2 * p_2_pw2 * p_1_pw2 - p_1_pw4 - 2 * f_2_pw2 * p_2_pw2 * p_1 * d_12
+ p_2_pw2 * f_1_pw2 * p_1_pw2 + f_2_pw2 * p_2_pw2 * d_12_pw2 * b_pw2;
// Computation of roots
Eigen::Matrix<double, 4, 1> realRoots;
P3P::solveQuartic(factors, realRoots);
// Backsubstitution of each solution
for (int i = 0; i < 4; ++i)
{
double cot_alpha = (-f_1 * p_1 / f_2 - realRoots(i) * p_2 + d_12 * b)
/ (-f_1 * realRoots(i) * p_2 / f_2 + p_1 - d_12);
double cos_theta = realRoots(i);
double sin_theta = sqrt(1 - pow((double)realRoots(i), 2));
double sin_alpha = sqrt(1 / (pow(cot_alpha, 2) + 1));
double cos_alpha = sqrt(1 - pow(sin_alpha, 2));
if (cot_alpha < 0)
{
cos_alpha = -cos_alpha;
}
Eigen::Vector3d C;
C(0) = d_12 * cos_alpha * (sin_alpha * b + cos_alpha);
C(1) = cos_theta * d_12 * sin_alpha * (sin_alpha * b + cos_alpha);
C(2) = sin_theta * d_12 * sin_alpha * (sin_alpha * b + cos_alpha);
C = P1 + N.transpose() * C;
Eigen::Matrix3d R;
R(0, 0) = -cos_alpha;
R(0, 1) = -sin_alpha * cos_theta;
R(0, 2) = -sin_alpha * sin_theta;
R(1, 0) = sin_alpha;
R(1, 1) = -cos_alpha * cos_theta;
R(1, 2) = -cos_alpha * sin_theta;
R(2, 0) = 0;
R(2, 1) = -sin_theta;
R(2, 2) = cos_theta;
R = N.transpose() * R.transpose() * T;
Eigen::Matrix<double, 3, 4> solution;
solution.block<3, 3>(0, 0) = R;
solution.col(3) = C;
solutions(i) = solution;
}
return 0;
}
opengv::translation_t
opengv::absolute_pose::p2p(
const AbsoluteAdapterBase & adapter,
size_t index0,
size_t index1)
{
Eigen::Vector3d e1 = adapter.getBearingVector(index0);
Eigen::Vector3d e3 = adapter.getBearingVector(index1);
e3 = e1.cross(e3);
e3 = e3/e3.norm();
Eigen::Vector3d e2 = e3.cross(e1);
rotation_t T;
T.row(0) = e1.transpose();
T.row(1) = e2.transpose();
T.row(2) = e3.transpose();
Eigen::Vector3d n1 = adapter.getPoint(index1) - adapter.getPoint(index0);
n1 = n1/n1.norm();
Eigen::Vector3d n3;
if( (fabs(n1[0]) > fabs(n1[1])) && (fabs(n1[0]) > fabs(n1[2])) )
{
n3[1] = 1.0;
n3[2] = 0.0;
n3[0] = -n1[1]/n1[0];
}
else
{
if( (fabs(n1[1]) > fabs(n1[0])) && (fabs(n1[1]) > fabs(n1[2])) )
{
n3[2] = 1.0;
n3[0] = 0.0;
n3[1] = -n1[2]/n1[1];
}
else
{
n3[0] = 1.0;
n3[1] = 0.0;
n3[2] = -n1[0]/n1[2];
}
}
n3 = n3 / n3.norm();
Eigen::Vector3d n2 = n3.cross(n1);
rotation_t N;
N.row(0) = n1.transpose();
N.row(1) = n2.transpose();
N.row(2) = n3.transpose();
Eigen::Matrix3d Q = T * adapter.getR().transpose() * N.transpose();
Eigen::Vector3d temp1 = adapter.getPoint(index1) - adapter.getPoint(index0);
double d_12 = temp1.norm();
Eigen::Vector3d temp2 = adapter.getBearingVector(index1);
double cos_beta = e1.dot(temp2);
double b = 1/( 1 - pow( cos_beta, 2 ) ) - 1;
if( cos_beta < 0 )
b = -sqrt(b);
else
b = sqrt(b);
double temp3 = d_12 * ( Q(1,0) * b - Q(0,0) );
translation_t solution = -temp3 * Q.row(0).transpose();
solution = adapter.getPoint(index0) + N.transpose()*solution;
if(
solution(0,0) != solution(0,0) ||
solution(1,0) != solution(1,0) ||
solution(2,0) != solution(2,0) )
solution = Eigen::Vector3d::Zero();
return solution;
}
int Hsh::loadData(FILE* file, int width, int height, int basisTerm, bool urti, CallBackPos * cb,const QString& text)
{
type = "HSH";
w = width;
h = height;
ordlen = basisTerm;
bands = 3;
fread(gmin, sizeof(float), basisTerm, file);
fread(gmax, sizeof(float), basisTerm, file);
if (feof(file))
return -1;
int offset = 0;
int size = w * h * basisTerm;
float* redPtr = new float[size];
float* greenPtr = new float[size];
float* bluePtr = new float[size];
unsigned char c;
if (!urti)
{
for(int j = 0; j < h; j++)
{
if (cb != NULL && j % 50 == 0)(*cb)(j * 50.0 / h, text);
for(int i = 0; i < w; i++)
{
offset = j * w + i;
for (int k = 0; k < basisTerm; k++)
{
if (feof(file))
return -1;
fread(&c, sizeof(unsigned char), 1, file);
redPtr[offset*basisTerm + k] = (((float)c) / 255.0) * (gmax[k] - gmin[k]) + gmin[k];
}
for (int k = 0; k < basisTerm; k++)
{
if (feof(file))
return -1;
fread(&c, sizeof(unsigned char), 1, file);
greenPtr[offset*basisTerm + k] = (((float)c) / 255.0) * (gmax[k] - gmin[k]) + gmin[k];
}
for (int k = 0; k < basisTerm; k++)
{
if (feof(file))
return -1;
fread(&c, sizeof(unsigned char), 1, file);
bluePtr[offset*basisTerm + k] = (((float)c) / 255.0) * (gmax[k] - gmin[k]) + gmin[k];
}
}
}
}
else
{
for(int j = 0; j < h; j++)
{
if (cb != NULL && j % 50 == 0)(*cb)(j * 50 / h, text);
for(int i = 0; i < w; i++)
{
offset = j * w + i;
for (int k = 0; k < basisTerm; k++)
{
if (feof(file))
return -1;
fread(&c, sizeof(unsigned char), 1, file);
redPtr[offset*basisTerm + k] = (((float)c) / 255.0) * gmin[k] + gmax[k];
}
for (int k = 0; k < basisTerm; k++)
{
if (feof(file))
return -1;
fread(&c, sizeof(unsigned char), 1, file);
greenPtr[offset*basisTerm + k] = (((float)c) / 255.0) * gmin[k] + gmax[k];
}
for (int k = 0; k < basisTerm; k++)
{
if (feof(file))
return -1;
fread(&c, sizeof(unsigned char), 1, file);
bluePtr[offset*basisTerm + k] = (((float)c) / 255.0) * gmin[k] + gmax[k];
}
}
}
}
fclose(file);
mipMapSize[0] = QSize(w, h);
redCoefficients.setLevel(redPtr, size, 0);
greenCoefficients.setLevel(greenPtr, size, 0);
blueCoefficients.setLevel(bluePtr, size, 0);
// Computes mip-mapping.
if (cb != NULL) (*cb)(50, "Mip mapping generation...");
for (int level = 1; level < MIP_MAPPING_LEVELS; level++)
{
int width = mipMapSize[level - 1].width();
int height = mipMapSize[level - 1].height();
int width2 = ceil(width / 2.0);
int height2 = ceil(height / 2.0);
size = width2*height2*basisTerm;
redCoefficients.setLevel(new float[size], size, level);
greenCoefficients.setLevel(new float[size], size, level);
blueCoefficients.setLevel(new float[size], size, level);
int th_id;
#pragma omp parallel for
for (int i = 0; i < height - 1; i+=2)
{
th_id = omp_get_thread_num();
if (th_id == 0)
{
if (cb != NULL && i % 50 == 0) (*cb)(50 + (level-1)*8 + i*8.0/height, "Mip mapping generation...");
}
for (int j = 0; j < width - 1; j+=2)
{
int index1 = (i * width + j)*ordlen;
int index2 = (i * width + j + 1)*ordlen;
int index3 = ((i + 1) * width + j)*ordlen;
int index4 = ((i + 1) * width + j + 1)*ordlen;
int offset = (i/2 * width2 + j/2)*ordlen;
for (int k = 0; k < basisTerm; k++)
{
redCoefficients.calcMipMapping(level, offset + k, index1 + k, index2 + k, index3 + k , index4 + k);
greenCoefficients.calcMipMapping(level, offset + k, index1 + k, index2 + k, index3 + k , index4 + k);
blueCoefficients.calcMipMapping(level, offset + k, index1 + k, index2 + k, index3 + k , index4 + k);
}
}
}
if (width2 % 2 != 0)
{
for (int i = 0; i < height - 1; i+=2)
{
int index1 = ((i + 1) * width - 1)*ordlen;
int index2 = ((i + 2) * width - 1)*ordlen;
int offset = ((i/2 + 1) * width2 - 1)*ordlen;
for (int k = 0; k < basisTerm; k++)
{
redCoefficients.calcMipMapping(level, offset + k, index1 + k, index2 + k);
greenCoefficients.calcMipMapping(level, offset + k, index1 + k, index2 + k);
blueCoefficients.calcMipMapping(level, offset + k, index1 + k, index2 + k);
}
}
}
if (height % 2 != 0)
{
for (int i = 0; i < width - 1; i+=2)
{
int index1 = ((height - 1) * width + i)*ordlen;
int index2 = ((height - 1) * width + i + 1)*ordlen;
int offset = ((height2 - 1) * width2 + i/2)*ordlen;
for (int k = 0; k < basisTerm; k++)
{
redCoefficients.calcMipMapping(level, offset + k, index1 + k, index2 + k);
greenCoefficients.calcMipMapping(level, offset + k, index1 + k, index2 + k);
blueCoefficients.calcMipMapping(level, offset + k, index1 + k, index2 + k);
}
}
}
if (height % 2 != 0 && width % 2 != 0)
{
int index1 = (height*width - 1)*ordlen;
int offset = (height2*width2 - 1)*ordlen;
for (int k = 0; k < basisTerm; k++)
{
redCoefficients.calcMipMapping(level, offset + k, index1 + k);
greenCoefficients.calcMipMapping(level, offset + k, index1 + k);
blueCoefficients.calcMipMapping(level, offset + k, index1 + k);
}
}
mipMapSize[level] = QSize(width2, height2);
}
//Compute normals.
if (cb != NULL) (*cb)(75 , "Normals generation...");
Eigen::Vector3d l0(sin(M_PI/4)*cos(M_PI/6), sin(M_PI/4)*sin(M_PI/6), cos(M_PI/4));
Eigen::Vector3d l1(sin(M_PI/4)*cos(5*M_PI / 6), sin(M_PI/4)*sin(5*M_PI / 6), cos(M_PI/4));
Eigen::Vector3d l2(sin(M_PI/4)*cos(3*M_PI / 2), sin(M_PI/4)*sin(3*M_PI / 2), cos(M_PI/4));
float hweights0[16], hweights1[16], hweights2[16];
getHSH(M_PI / 4, M_PI / 6, hweights0, ordlen);
getHSH(M_PI / 4, 5*M_PI / 6, hweights1, ordlen);
getHSH(M_PI / 4, 3*M_PI / 2, hweights2, ordlen);
Eigen::Matrix3d L;
L.setIdentity();
L.row(0) = l0;
L.row(1) = l1;
L.row(2) = l2;
Eigen::Matrix3d LInverse = L.inverse();
for (int level = 0; level < MIP_MAPPING_LEVELS; level++)
{
const float* rPtr = redCoefficients.getLevel(level);
const float* gPtr = greenCoefficients.getLevel(level);
const float* bPtr = blueCoefficients.getLevel(level);
vcg::Point3f* temp = new vcg::Point3f[mipMapSize[level].width()*mipMapSize[level].height()];
if (cb != NULL) (*cb)(75 + level*6, "Normal generation...");
#pragma omp parallel for
for (int y = 0; y < mipMapSize[level].height(); y++)
{
for (int x = 0; x < mipMapSize[level].width(); x++)
{
int offset= y * mipMapSize[level].width() + x;
Eigen::Vector3d f(0, 0, 0);
for (int k = 0; k < ordlen; k++)
{
f(0) += rPtr[offset*ordlen + k] * hweights0[k];
f(1) += rPtr[offset*ordlen + k] * hweights1[k];
f(2) += rPtr[offset*ordlen + k] * hweights2[k];
}
for (int k = 0; k < ordlen; k++)
{
f(0) += gPtr[offset*ordlen + k] * hweights0[k];
f(1) += gPtr[offset*ordlen + k] * hweights1[k];
f(2) += gPtr[offset*ordlen + k] * hweights2[k];
}
for (int k = 0; k < ordlen; k++)
{
f(0) += bPtr[offset*ordlen + k] * hweights0[k];
f(1) += bPtr[offset*ordlen + k] * hweights1[k];
f(2) += bPtr[offset*ordlen + k] * hweights2[k];
}
f /= 3.0;
Eigen::Vector3d normal = LInverse * f;
temp[offset] = vcg::Point3f(normal(0), normal(1), normal(2));
temp[offset].Normalize();
}
}
normals.setLevel(temp, mipMapSize[level].width()*mipMapSize[level].height(), level);
}
return 0;
}
int main(int argc, char* argv[]) {
google::InitGoogleLogging(argv[0]);
//Prepare files to run Kyle's 1DSFM Code
FILE* file1 = fopen("../output/cc.txt", "w");
if(file1 == NULL) {
printf("\nCould not open CC file to write");
return -1;
}
FILE* file2 = fopen("../output/EGs.txt", "w");
if(file2 == NULL) {
printf("\nCould not open EG file to write");
return -1;
}
FILE* file3 = fopen("../output/coords.txt", "w");
if(file2 == NULL) {
printf("\nCould not open coords file to write");
return -1;
}
FILE* file4 = fopen("../output/verified_matches.txt", "w");
if(file4 == NULL) {
printf("\nCould not open tracks file to write");
return -1;
}
/*
for(int i=0; i < numKeys; i++) {
fprintf(file1,"%d\n",i);
}*/
vector<string> imnames;
vector<CameraIntrinsicsPrior> cps;
vector<ImagePairMatch> matches;
bool s0 = ReadMatchesAndGeometry("../data/view_graph.bin",&imnames,&cps,&matches);
if(s0) {
printf("\nSuccessfully Read Matches And Geo, matches size %d", matches.size());
for(int i=0; i < matches.size(); i++) {
//for(int i=0; i < 5; i++) {
int image1 = matches[i].image1_index;
int image2 = matches[i].image2_index;
TwoViewInfo currPairInfo = matches[i].twoview_info;
double focal_length1 = currPairInfo.focal_length_1;
double focal_length2 = currPairInfo.focal_length_2;
Eigen::Vector3d rotation = currPairInfo.rotation_2;
Eigen::Vector3d position = currPairInfo.position_2;
double angleMag = rotation.norm();
Eigen::Vector3d axisVec = rotation.normalized();
Eigen::AngleAxisd angax_rotation(angleMag, axisVec);
Eigen::Matrix3d rotationMat = angax_rotation.toRotationMatrix();
//std::cout << "Matrix " << i << ", rows" << rotationMat.rows() << ", cols" << rotationMat.cols() << endl;
//std::cout << rotationMat;
fprintf(file2, "%d %d ", image1, image2);
for(int r= 0; r < 3; r++) {
Eigen::Vector3d row_i = rotationMat.row(r);
fprintf(file2, "%f %f %f ", row_i[0], row_i[1], row_i[2]);
}
fprintf(file2, "%f %f %f\n", position[0], position[1], position[2]);
}
fclose(file1);
fclose(file2);
fclose(file3);
fclose(file4);
} else {
printf("\nCould not read bin file");
return -1;
}
return 0;
}
int main(int argc, char* argv[]) {
// Initialize logging to --log_dir
google::InitGoogleLogging(argv[0]);
// Read list files with image paths, image dims and key paths
// argv[1] stores image_dims.txt, list_keys.txt, list_images.txt
string imgListFile = argv[1];
string keyListFile = argv[1];
string intrinsicFileName = string(argv[1]) + "/intrinsics.txt";
// Read keyfile names
// Read image file names and dimensions
reader::ImageListReader imgList(imgListFile);
reader::KeyListReader keyList(keyListFile);
bool s1 = imgList.read();
bool s2 = keyList.read();
if(!s1 || !s2) {
printf("\nError reading list file or key file");
return 0;
}
// Open file that stores focal lengths and radial distortion params
// Find all nearby images based on ranks
ifstream intrFile(intrinsicFileName.c_str());
if(!intrFile.is_open()) {
cout << "\nError opening intr File";
return -1;
}
string line;
vector<double> precompFocals;
while(getline(intrFile, line)) {
istringstream str(line);
double a, b,c;
str >> a >> b >> c;
precompFocals.push_back(a);
}
// Open files to write pairwise matching output
// As per Kyle Wilson's 1D SFM package's input requirements
FILE* file1 = fopen("../output/cc.txt", "w");
if(file1 == NULL) {
printf("\nCould not open CC file to write");
return -1;
}
FILE* file2 = fopen("../output/EGs.txt", "w");
if(file2 == NULL) {
printf("\nCould not open EG file to write");
return -1;
}
FILE* file3 = fopen("../output/coords.txt", "w");
if(file2 == NULL) {
printf("\nCould not open coords file to write");
return -1;
}
FILE* file4 = fopen("../output/input_tracks.txt", "w");
if(file4 == NULL) {
printf("\nCould not open tracks file to write");
return -1;
}
// Initialize containers for various data
int numKeys = keyList.getNumKeys();
vector< unsigned char* > keys(numKeys);
vector< keypt_t* > keysInfo(numKeys);
vector< int > numFeatures(numKeys);
vector<string> view_names(numKeys);
vector<double> halfWidth(numKeys);
vector<double> halfHeight(numKeys);
vector<double> focalLengths(numKeys, 1.0f);
vector<bool> calibrationFlag(numKeys, false);
vector<CameraIntrinsicsPrior> camPriors;
//Create a single ExifReader object to extract focal lengths
ExifReader exReader;
for(int i=0; i < keyList.getNumKeys(); i++) {
printf("\nReading keyfile %d/%d", i, numKeys);
string keyPath = keyList.getKeyName(i);
numFeatures[i] = ReadKeyFile(keyPath.c_str(),
&keys[i], &keysInfo[i]);
int W = imgList.getImageWidth(i);
int H = imgList.getImageHeight(i);
halfWidth[i] = (double)W/2.0f;
halfHeight[i] = (double)H/2.0f;
// Get image name (abc.jpg) if full path is given
string path = imgList.getImageName(i);
size_t sep = path.find_last_of("\\/");
if (sep != std::string::npos)
path = path.substr(sep + 1, path.size() - sep - 1);
view_names[i] = path;
// Read EXIF data find focal length
printf("\nReading exif data %d/%d", i, numKeys);
CameraIntrinsicsPrior cPrior, dummyPrior;
bool status = exReader.ExtractEXIFMetadata(imgList.getImageName(i), &cPrior);
if(status) {
printf("\nRead EXIF data:");
if(cPrior.focal_length.is_set) {
calibrationFlag[i] = true;
focalLengths[i] = cPrior.focal_length.value;
camPriors.push_back(cPrior);
} else if(precompFocals[i] != 0) {
calibrationFlag[i] = true;
focalLengths[i] = precompFocals[i];
camPriors.push_back(cPrior);
} else {
camPriors.push_back(dummyPrior);
}
} else {
camPriors.push_back(dummyPrior);
}
}
// Read matches file and veriy two view matches
FILE* fp = fopen( argv[2] , "r");
if( fp == NULL ) {
printf("\nCould not read matches file");
fflush(stdout);
return 0;
}
int img1, img2;
vector<ImagePairMatch> matches;
while(fscanf(fp,"%d %d",&img1,&img2) != EOF) {
vector< pair<int,int> > matchIdxPairs;
vector<FeatureCorrespondence> featCorrs;
vector<FeatureCorrespondence> inlFeatCorrs;
int numMatches;
fscanf(fp,"%d",&numMatches);
fflush(stdout);
for(int i=0; i < numMatches; i++) {
int matchIdx1, matchIdx2;
fscanf(fp,"%d %d",&matchIdx1, &matchIdx2);
float x1 = keysInfo[img1][matchIdx1].x;
float y1 = keysInfo[img1][matchIdx1].y;
float x2 = keysInfo[img2][matchIdx2].x;
float y2 = keysInfo[img2][matchIdx2].y;
Feature f1(x1, y1);
Feature f2(x2, y2);
FeatureCorrespondence corres;
corres.feature1 = f1;
corres.feature2 = f2;
featCorrs.push_back(corres);
matchIdxPairs.push_back(make_pair(matchIdx1, matchIdx2));
}
vector<int> inliers;
TwoViewInfo currPairInfo;
VerifyTwoViewMatchesOptions options;
bool status = VerifyTwoViewMatches(options,
camPriors[img1], camPriors[img2],
featCorrs,&currPairInfo,&inliers);
if(status) {
printf("\nSuccessfully Verified Matches");
printf("\nFocal Length 1 = %f, Focal Length 2 = %f", camPriors[img1].focal_length.value, camPriors[img2].focal_length.value);
printf("\nFocal Length 1 = %f, Focal Length 2 = %f", currPairInfo.focal_length_1, currPairInfo.focal_length_2);
for(int inl_id=0; inl_id < inliers.size(); inl_id++) {
inlFeatCorrs.push_back( featCorrs[inliers[inl_id]] );
}
ImagePairMatch imPair;
imPair.image1_index = img1;
imPair.image2_index = img2;
imPair.twoview_info = currPairInfo;
imPair.correspondences = inlFeatCorrs;
matches.push_back(imPair);
int numInl = inliers.size();
// fprintf(file4, "%d\n", inliers.size());
for(int j=0; j < numInl; j++) {
pair<int,int> p = matchIdxPairs[inliers[j]];
fprintf(file4, "2 %d %d %d %d\n", img1, p.first, img2, p.second);
}
}
}
bool status = WriteMatchesAndGeometry("view_graph.bin", view_names, camPriors, matches);
if(!status) {
printf("\nSuccessfully written view-graph file");
}
// Write remaining files for Kyle's 1dsfm
for(int i=0; i < numKeys; i++) {
fprintf(file1,"%d\n",i);
fprintf(file3, "#index = %d, name = %s, keys = %d ",
i, imgList.getImageName(i).c_str(), numFeatures[i]);
fprintf(file3, "px = %.1f, py = %.1f, focal = %.3f\n",
halfWidth[i], halfHeight[i], focalLengths[i]);
for(int j=0; j < numFeatures[i]; j++) {
fprintf(file3, "%d %f %f 0 0 %d %d %d\n",
j, keysInfo[i][j].x, keysInfo[i][j].y,
0,0,0);
}
}
for(int i=0; i < matches.size(); i++) {
int image1 = matches[i].image1_index;
int image2 = matches[i].image2_index;
TwoViewInfo currPairInfo = matches[i].twoview_info;
double focal_length1 = currPairInfo.focal_length_1;
double focal_length2 = currPairInfo.focal_length_2;
Eigen::Vector3d rotation = currPairInfo.rotation_2;
Eigen::Vector3d position = currPairInfo.position_2;
double angleMag = rotation.norm();
Eigen::Vector3d axisVec = rotation.normalized();
Eigen::AngleAxisd angax_rotation(angleMag, axisVec);
Eigen::Matrix3d rotationMat = angax_rotation.toRotationMatrix();
fprintf(file2, "%d %d ", image1, image2);
for(int r= 0; r < 3; r++) {
Eigen::Vector3d row_i = rotationMat.row(r);
fprintf(file2, "%f %f %f ", row_i[0], row_i[1], row_i[2]);
}
fprintf(file2, "%f %f %f\n", position[0], position[1], position[2]);
}
fclose(file1);
fclose(file2);
fclose(file3);
fclose(file4);
return 0;
}
