const std::vector<Lines<float> > Algorithms::normalizeLines(const std::vector<Lines<int> > lines, int &minX, int &maxX, int &minY, int &maxY){
/* // Set initial values to extremes of the opposites
int minX = INT_MAX;
int maxX = INT_MIN;
int minY = INT_MAX;
int maxY = INT_MIN;
// Find the minimum and maximum x and y for the range of the map
for(std::vector<Line<int> >::const_iterator it = lines.begin(); it != lines.end(); ++it) {
if (minX > it->x1) {
minX = it->x1;
}
if (minX > it->x2) {
minX = it->x2;
}
if (maxX < it->x1) {
maxX = it->x1;
}
if (maxX < it->x2) {
maxX = it->x2;
}
if (minY > it->y1) {
minY = it->y1;
}
if (minY > it->y2) {
minY = it->y2;
}
if (maxY < it->y1) {
maxY = it->y1;
}
if (maxY < it->y2) {
maxY = it->y2;
}
}*/
int difX = maxX - minX;
int difY = maxY - minY;
// return const std::vector<Line> linesNormalized
std::vector<Lines<float> > linesNormalized;
for(std::vector<Lines<int> >::const_iterator it = lines.begin(); it != lines.end(); ++it) {
// Normalize x and y between -1.0 and 1.0
float x1 = ((float(it->x1 - minX) / difX) * DIF_X) + MIN_X;
float y1 = ((float(it->y1 - minY) / difY) * DIF_Y) + MIN_Y;
float x2 = ((float(it->x2 - minX) / difX) * DIF_X) + MIN_X;
float y2 = ((float(it->y2 - minY) / difY) * DIF_Y) + MIN_Y;
Lines<float> lineNormalized(x1, y1, x2, y2);
linesNormalized.push_back(lineNormalized);
}
return linesNormalized;
}
const std::vector<Lines<float> > Algorithms::getNormalizedLinesForPolygons(const std::vector<std::vector<points> >& polygons) {
std::vector<std::vector<points> >::const_iterator it_polygons;
std::vector<points>::const_iterator it;
// Set initial values to extremes of the opposites
int minX = INT_MAX;
int maxX = INT_MIN;
int minY = INT_MAX;
int maxY = INT_MIN;
// Find the minimum and maximum x and y for the range of the map
for(it_polygons = polygons.begin(); it_polygons != polygons.end(); ++it_polygons) {
for(it = (*it_polygons).begin(); it != (*it_polygons).end(); ++it) {
if (minX > it->getX()) {
minX = it->getX();
}
if (maxX < it->getX()) {
maxX = it->getX();
}
if (minY > it->getY()) {
minY = it->getY();
}
if (maxY < it->getY()) {
maxY = it->getY();
}
}
}
int difX = maxX - minX;
int difY = maxY - minY;
std::vector<Lines<float> > linesNormalized;
for(it_polygons = polygons.begin(); it_polygons != polygons.end(); ++it_polygons) {
points *prev = NULL;
for(it = (*it_polygons).begin(); it != (*it_polygons).end(); ++it) {
if (prev != NULL) {
// Normalize x and y between -1.0 and 1.0
float x1 = ((float(prev->getX() - minX) / difX) * DIF_X) + MIN_X;
float y1 = ((float(prev->getY() - minY) / difY) * DIF_Y) + MIN_Y;
float x2 = ((float(it->getX() - minX) / difX) * DIF_X) + MIN_X;
float y2 = ((float(it->getY() - minY) / difY) * DIF_Y) + MIN_Y;
Lines<float> lineNormalized(x1, y1, x2, y2);
linesNormalized.push_back(lineNormalized);
}
*prev = *it;
}
}
return linesNormalized;
}
int main(int argc, char* argv[])
{
#if 1
QApplication a(argc, argv);
MainWindow w;
w.show();
// look for image file
//std::string img_file("data/floor.jpg");
//QFile file;
//for (int i = 0; i < 5; ++i)
// if (!file.exists(img_file.c_str()))
// img_file.insert(0, "../");
//w.getGLWidget()->setImage(img_file.c_str());
GLLines* gllines = &w.getGLWidget()->glLines();
//gllines->setVertexLine(0, 0, QVector3D(52.3467f, 125.102f, 1.0f));
//gllines->setVertexLine(0, 1, QVector3D(340.253f, 130.147f, 1.0f));
//gllines->setVertexLine(1, 0, QVector3D(193.28f, 126.111f, 1.0f));
//gllines->setVertexLine(1, 1, QVector3D(225.493f, 360.173f, 1.0f));
//gllines->setVertexLine(2, 0, QVector3D(42.28f, 263.32f, 1.0f));
//gllines->setVertexLine(2, 1, QVector3D(296.967f, 397.502f, 1.0f));
//gllines->setVertexLine(3, 0, QVector3D(212.407f, 269.373f, 1.0f));
//gllines->setVertexLine(3, 1, QVector3D(34.2267f, 391.449f, 1.0f));
//gllines->setVertexLine(4, 0, QVector3D(294.953f, 318.809f, 1.0f));
//gllines->setVertexLine(4, 1, QVector3D(456.02f, 322.844f, 1.0f));
//gllines->setVertexLine(5, 0, QVector3D(492.26f, 208.84f, 1.0f));
//gllines->setVertexLine(5, 1, QVector3D(429.847f, 400.529f, 1.0f));
//gllines->setVertexLine(6, 0, QVector3D(299.987f, 31.2756f, 1.0f));
//gllines->setVertexLine(6, 1, QVector3D(555.68f, 273.409f, 1.0f));
//gllines->setVertexLine(7, 0, QVector3D(545.613f, 39.3467f, 1.0f));
//gllines->setVertexLine(7, 1, QVector3D(236.567f, 250.204f, 1.0f));
//gllines->setVertexLine(8, 0, QVector3D(95.6333f, 264.329f, 1.0f));
//gllines->setVertexLine(8, 1, QVector3D(501.32f, 273.409f, 1.0f));
//gllines->setVertexLine(9, 0, QVector3D(302.00f, 29.2578f, 1.0f));
//gllines->setVertexLine(9, 1, QVector3D(297.973f, 398.511f, 1.0f));
gllines->computeCanonicalVertices(w.getGLWidget()->width(), w.getGLWidget()->height());
gllines->onEnable(true);
return a.exec();
#else
std::vector<Eigen::Vector3f> vertices;
vertices.push_back(Eigen::Vector3f(52.3467f, 125.102f, 1.0f));
vertices.push_back(Eigen::Vector3f(340.253f, 130.147f, 1.0f));
vertices.push_back(Eigen::Vector3f(193.28f, 126.111f, 1.0f));
vertices.push_back(Eigen::Vector3f(225.493f, 360.173f, 1.0f));
vertices.push_back(Eigen::Vector3f(42.28f, 263.32f, 1.0f));
vertices.push_back(Eigen::Vector3f(296.967f, 397.502f, 1.0f));
vertices.push_back(Eigen::Vector3f(212.407f, 269.373f, 1.0f));
vertices.push_back(Eigen::Vector3f(34.2267f, 391.449f, 1.0f));
vertices.push_back(Eigen::Vector3f(294.953f, 318.809f, 1.0f));
vertices.push_back(Eigen::Vector3f(456.02f, 322.844f, 1.0f));
vertices.push_back(Eigen::Vector3f(492.26f, 208.84f, 1.0f));
vertices.push_back(Eigen::Vector3f(429.847f, 400.529f, 1.0f));
vertices.push_back(Eigen::Vector3f(299.987f, 31.2756f, 1.0f));
vertices.push_back(Eigen::Vector3f(555.68f, 273.409f, 1.0f));
vertices.push_back(Eigen::Vector3f(545.613f, 39.3467f, 1.0f));
vertices.push_back(Eigen::Vector3f(236.567f, 250.204f, 1.0f));
vertices.push_back(Eigen::Vector3f(95.6333f, 264.329f, 1.0f));
vertices.push_back(Eigen::Vector3f(501.32f, 273.409f, 1.0f));
vertices.push_back(Eigen::Vector3f(302.00f, 29.2578f, 1.0f));
vertices.push_back(Eigen::Vector3f(297.973f, 398.511f, 1.0f));
std::cout << vertices.size() << std::endl;
std::vector<Eigen::Vector3f> lines;
for (int i = 0; i < vertices.size() - 1; i += 2)
lines.push_back(lineNormalized(vertices[i], vertices[i + 1]));
for (int i = 0; i < lines.size(); ++i)
std::cout << "l" << i << " : " << lines[i].x() << ", " << lines[i].y() << ", " << lines[i].z() << std::endl;
std::cout << std::endl << std::endl;
//lines[0] = Eigen::Vector3f(-0.000141084, 0.00805224, -0.999968);
//lines[1] = Eigen::Vector3f(-0.00568419, 0.000782299, 0.999984);
//lines[2] = Eigen::Vector3f(-0.00218568, 0.00414856, -0.999989);
//lines[3] = Eigen::Vector3f(-0.0016513, -0.00241022, 0.999996);
//lines[4] = Eigen::Vector3f(-8.04546e-05, 0.00321109, -0.999995);
//lines[5] = Eigen::Vector3f(-0.00178489, -0.000581155, 0.999998);
//lines[6] = Eigen::Vector3f(-0.00374583, 0.0039556, 0.999985);
//lines[7] = Eigen::Vector3f(-0.00165759, -0.00242947, 0.999996);
//lines[8] = Eigen::Vector3f(-8.53647e-05, 0.00381402, -0.999993);
//lines[9] = Eigen::Vector3f(-0.00330775, -3.60706e-05, 0.999995);
Eigen::MatrixXf A(5, 5);
Eigen::Vector3f l, m;
Eigen::VectorXf b(5);
for (int i = 0; i < 5; ++i)
{
l = lines[i * 2 + 0];
m = lines[i * 2 + 1];
A(i, 0) = l[0] * m[0];
A(i, 1) = (l[0] * m[1] + l[1] * m[0]) / 2.0f;
A(i, 2) = l[1] * m[1];
A(i, 3) = (l[0] * m[2] + l[2] * m[0]) / 2.0f;
A(i, 4) = (l[1] * m[2] + l[2] * m[1]) / 2.0f;
A(i, 5) = l[2] * m[2];
b[i] = -l[2] * m[2];
}
std::cout << "A: \n" << A << std::endl << std::endl;
std::cout << "b: \n" << b << std::endl << std::endl;
Eigen::MatrixXf x = A.colPivHouseholderQr().solve(b);
std::cout << std::fixed << "x: \n" << x << std::endl << std::endl;
Eigen::MatrixXf conic(3, 3);
conic(0, 0) = x(0);
conic(0, 1) = x(1) / 2.0f;
conic(0, 2) = x(3) / 2.0f;
conic(1, 0) = x(1) / 2.0f;
conic(1, 1) = x(2);
conic(1, 2) = x(4) / 2.0f;
conic(2, 0) = x(3) / 2.0f;
conic(2, 1) = x(4) / 2.0f;
conic(2, 2) = 1.0f;
std::cout << "Conic : " << std::endl << conic << std::endl << std::endl;
Eigen::JacobiSVD<Eigen::MatrixXf> svd(conic, Eigen::ComputeFullU);
Eigen::MatrixXf H = svd.matrixU();
std::cout << "H matrix: " << std::endl
<< H << std::endl << std::endl
<< "Singular values: " << svd.singularValues()
<< std::endl << std::endl;
std::cout << "Rectification transformation: " << std::endl << H.inverse() << std::endl << std::endl;
QImage input("floor-persp.jpg");
Eigen::Vector3f img(input.width(), input.height(), 1.0f);
float xmin = 0;
float xmax = 0;
float ymin = 0;
float ymax = 0;
computImageSize(H.inverse(), 0, 0, input.width(), input.height(), xmin, xmax, ymin, ymax);
float aspect = (xmax - xmin) / (ymax - ymin);
QImage output(input.width(), input.width() / aspect, input.format());
output.fill(qRgb(0, 0, 0));
std::cout << "Output size: " << output.width() << ", " << output.height() << std::endl;
float dx = (xmax - xmin) / float(output.width());
float dy = (ymax - ymin) / float(output.height());
std::cout << std::fixed << "dx, dy: " << dx << ", " << dy << std::endl;
for (int x = 0; x < output.width(); ++x)
{
for (int y = 0; y < output.height(); ++y)
{
Eigen::Vector3f px(x, y, 1);
float tx = 0.0f;
float ty = 0.0f;
Eigen::Vector2f t = multiplyPointMatrix(H, xmin + x * dx, ymin + y * dy);
if (t.x() > -1 && t.y() > -1
&& t.x() < input.width()
&& t.y() < input.height())
{
//if (interpolate)
//{
// QRgb rgb = bilinearInterpol(input, t.x(), t.y(), dx / 2.0, dy / 2.0);
// output.setPixel(x, y, rgb);
//}
//else
{
output.setPixel(x, y, input.pixel(t.x(), t.y()));
}
}
}
}
output.save("output_5_floor.jpg");
return EXIT_SUCCESS;
#endif
}
