kopia lustrzana https://github.com/OpenDroneMap/ODM
1553 wiersze
54 KiB
C++
1553 wiersze
54 KiB
C++
// C++
|
|
#include <math.h>
|
|
#include <sstream>
|
|
#include <fstream>
|
|
#include <Eigen/StdVector>
|
|
|
|
// This
|
|
#include "OdmOrthoPhoto.hpp"
|
|
|
|
std::ostream & operator<< (std::ostream &os, const WorldPoint &worldPoint)
|
|
{
|
|
return os << worldPoint.eastInteger_ + worldPoint.eastFractional_ << " " << worldPoint.northInteger_ + worldPoint.northFractional_;
|
|
}
|
|
|
|
std::istream & operator>> (std::istream &is, WorldPoint &worldPoint)
|
|
{
|
|
is >> worldPoint.eastInteger_;
|
|
// Check if east coordinate is given as rational.
|
|
if('.' == is.peek())
|
|
{
|
|
is >> worldPoint.eastFractional_;
|
|
}
|
|
else
|
|
{
|
|
worldPoint.eastFractional_ = 0.0f;
|
|
}
|
|
|
|
is >> worldPoint.northInteger_;
|
|
// Check if north coordinate is given as rational.
|
|
if('.' == is.peek())
|
|
{
|
|
is >> worldPoint.northFractional_;
|
|
}
|
|
else
|
|
{
|
|
worldPoint.northFractional_ = 0.0f;
|
|
}
|
|
|
|
return is;
|
|
}
|
|
|
|
OdmOrthoPhoto::OdmOrthoPhoto()
|
|
:log_(false)
|
|
{
|
|
inputFile_ = "";
|
|
inputGeoRefFile_ = "";
|
|
inputTransformFile_ = "";
|
|
outputFile_ = "ortho.jpg";
|
|
logFile_ = "log.txt";
|
|
outputCornerFile_ = "";
|
|
|
|
transformOverride_ = false;
|
|
|
|
resolution_ = 0.0f;
|
|
|
|
boundaryDefined_ = false;
|
|
boundaryPoint1_[0] = 0.0f; boundaryPoint1_[1] = 0.0f;
|
|
boundaryPoint2_[0] = 0.0f; boundaryPoint2_[1] = 0.0f;
|
|
boundaryPoint3_[0] = 0.0f; boundaryPoint3_[1] = 0.0f;
|
|
boundaryPoint4_[0] = 0.0f; boundaryPoint4_[1] = 0.0f;
|
|
}
|
|
|
|
OdmOrthoPhoto::~OdmOrthoPhoto()
|
|
{
|
|
}
|
|
|
|
int OdmOrthoPhoto::run(int argc, char *argv[])
|
|
{
|
|
try
|
|
{
|
|
parseArguments(argc, argv);
|
|
createOrthoPhoto();
|
|
}
|
|
catch (const OdmOrthoPhotoException& e)
|
|
{
|
|
log_.setIsPrintingInCout(true);
|
|
log_ << e.what() << "\n";
|
|
log_.print(logFile_);
|
|
return EXIT_FAILURE;
|
|
}
|
|
catch (const std::exception& e)
|
|
{
|
|
log_.setIsPrintingInCout(true);
|
|
log_ << "Error in OdmOrthoPhoto:\n";
|
|
log_ << e.what() << "\n";
|
|
log_.print(logFile_);
|
|
return EXIT_FAILURE;
|
|
}
|
|
catch (...)
|
|
{
|
|
log_.setIsPrintingInCout(true);
|
|
log_ << "Unknown error, terminating:\n";
|
|
log_.print(logFile_);
|
|
return EXIT_FAILURE;
|
|
}
|
|
|
|
log_.print(logFile_);
|
|
|
|
return EXIT_SUCCESS;
|
|
}
|
|
|
|
void OdmOrthoPhoto::parseArguments(int argc, char *argv[])
|
|
{
|
|
logFile_ = std::string(argv[0]) + "_log.txt";
|
|
log_ << logFile_ << "\n\n";
|
|
|
|
// If no arguments were passed, print help.
|
|
if (argc == 1)
|
|
{
|
|
printHelp();
|
|
}
|
|
|
|
log_ << "Arguments given\n";
|
|
for(int argIndex = 1; argIndex < argc; ++argIndex)
|
|
{
|
|
log_ << argv[argIndex] << '\n';
|
|
}
|
|
|
|
log_ << '\n';
|
|
for(int argIndex = 1; argIndex < argc; ++argIndex)
|
|
{
|
|
// The argument to be parsed.
|
|
std::string argument = std::string(argv[argIndex]);
|
|
|
|
if(argument == "-help")
|
|
{
|
|
printHelp();
|
|
}
|
|
else if(argument == "-resolution")
|
|
{
|
|
++argIndex;
|
|
if (argIndex >= argc)
|
|
{
|
|
throw OdmOrthoPhotoException("Argument '" + argument + "' expects 1 more input following it, but no more inputs were provided.");
|
|
}
|
|
std::stringstream ss(argv[argIndex]);
|
|
ss >> resolution_;
|
|
log_ << "Resolution count was set to: " << resolution_ << "pixels/meter\n";
|
|
}
|
|
else if(argument == "-boundary")
|
|
{
|
|
if(argIndex+8 >= argc)
|
|
{
|
|
throw OdmOrthoPhotoException("Argument '" + argument + "' expects 8 more input following it, but no more inputs were provided.");
|
|
}
|
|
|
|
std::stringstream ss;
|
|
ss << argv[argIndex+1] << " " << argv[argIndex+2] << " " << argv[argIndex+3] << " " << argv[argIndex+4] << " " << argv[argIndex+5] << " " << argv[argIndex+6] << " " << argv[argIndex+7] << " " << argv[argIndex+8];
|
|
ss >> worldPoint1_ >> worldPoint2_ >> worldPoint3_ >> worldPoint4_;
|
|
boundaryDefined_ = true;
|
|
|
|
argIndex += 8;
|
|
|
|
log_ << "Boundary point 1 was set to: " << worldPoint1_ << '\n';
|
|
log_ << "Boundary point 2 was set to: " << worldPoint2_ << '\n';
|
|
log_ << "Boundary point 3 was set to: " << worldPoint3_ << '\n';
|
|
log_ << "Boundary point 4 was set to: " << worldPoint4_ << '\n';
|
|
}
|
|
else if(argument == "-boundaryMinMax")
|
|
{
|
|
if(argIndex+4 >= argc)
|
|
{
|
|
throw OdmOrthoPhotoException("Argument '" + argument + "' expects 4 more input following it, but no more inputs were provided.");
|
|
}
|
|
|
|
std::stringstream ss;
|
|
ss << argv[argIndex+1] << " " << argv[argIndex+2] << " " << argv[argIndex+3] << " " << argv[argIndex+4];
|
|
ss >> worldPoint1_ >> worldPoint3_;
|
|
boundaryDefined_ = true;
|
|
|
|
// Set the other world points as the other two corners.
|
|
worldPoint2_.eastFractional_ = worldPoint1_.eastFractional_;
|
|
worldPoint2_.eastInteger_ = worldPoint1_.eastInteger_;
|
|
worldPoint2_.northFractional_ = worldPoint3_.northFractional_;
|
|
worldPoint2_.northInteger_ = worldPoint3_.northInteger_;
|
|
|
|
worldPoint4_.eastFractional_ = worldPoint3_.eastFractional_;
|
|
worldPoint4_.eastInteger_ = worldPoint3_.eastInteger_;
|
|
worldPoint4_.northFractional_ = worldPoint1_.northFractional_;
|
|
worldPoint4_.northInteger_ = worldPoint1_.northInteger_;
|
|
|
|
argIndex += 4;
|
|
|
|
log_ << "Boundary point 1 was set to: " << worldPoint1_ << '\n';
|
|
log_ << "Boundary point 2 was set to: " << worldPoint2_ << '\n';
|
|
log_ << "Boundary point 3 was set to: " << worldPoint3_ << '\n';
|
|
log_ << "Boundary point 4 was set to: " << worldPoint4_ << '\n';
|
|
}
|
|
else if(argument == "-verbose")
|
|
{
|
|
log_.setIsPrintingInCout(true);
|
|
}
|
|
else if (argument == "-logFile")
|
|
{
|
|
++argIndex;
|
|
if (argIndex >= argc)
|
|
{
|
|
throw OdmOrthoPhotoException("Missing argument for '" + argument + "'.");
|
|
}
|
|
logFile_ = std::string(argv[argIndex]);
|
|
std::ofstream testFile(logFile_.c_str());
|
|
if (!testFile.is_open())
|
|
{
|
|
throw OdmOrthoPhotoException("Argument '" + argument + "' has a bad value.");
|
|
}
|
|
log_ << "Log file path was set to: " << logFile_ << "\n";
|
|
}
|
|
else if(argument == "-inputFile")
|
|
{
|
|
argIndex++;
|
|
if (argIndex >= argc)
|
|
{
|
|
throw OdmOrthoPhotoException("Argument '" + argument + "' expects 1 more input following it, but no more inputs were provided.");
|
|
}
|
|
inputFile_ = std::string(argv[argIndex]);
|
|
log_ << "Reading textured mesh from: " << inputFile_ << "\n";
|
|
}
|
|
else if(argument == "-inputGeoRefFile")
|
|
{
|
|
argIndex++;
|
|
if (argIndex >= argc)
|
|
{
|
|
throw OdmOrthoPhotoException("Argument '" + argument + "' expects 1 more input following it, but no more inputs were provided.");
|
|
}
|
|
inputGeoRefFile_ = std::string(argv[argIndex]);
|
|
log_ << "Reading georef from: " << inputGeoRefFile_ << "\n";
|
|
}
|
|
else if(argument == "-outputFile")
|
|
{
|
|
argIndex++;
|
|
if (argIndex >= argc)
|
|
{
|
|
throw OdmOrthoPhotoException("Argument '" + argument + "' expects 1 more input following it, but no more inputs were provided.");
|
|
}
|
|
outputFile_ = std::string(argv[argIndex]);
|
|
log_ << "Writing output to: " << outputFile_ << "\n";
|
|
}
|
|
else if(argument == "-outputCornerFile")
|
|
{
|
|
argIndex++;
|
|
if (argIndex >= argc)
|
|
{
|
|
throw OdmOrthoPhotoException("Argument '" + argument + "' expects 1 more input following it, but no more inputs were provided.");
|
|
}
|
|
outputCornerFile_ = std::string(argv[argIndex]);
|
|
log_ << "Writing corners to: " << outputCornerFile_ << "\n";
|
|
}
|
|
else if(argument == "-inputTransformFile")
|
|
{
|
|
argIndex++;
|
|
if (argIndex >= argc)
|
|
{
|
|
throw OdmOrthoPhotoException("Argument '" + argument + "' expects 1 more input following it, but no more inputs were provided.");
|
|
}
|
|
inputTransformFile_ = std::string(argv[argIndex]);
|
|
transformOverride_ = true;
|
|
log_ << "Reading transformation matrix from: " << outputCornerFile_ << "\n";
|
|
}
|
|
else
|
|
{
|
|
printHelp();
|
|
throw OdmOrthoPhotoException("Unrecognised argument '" + argument + "'");
|
|
}
|
|
}
|
|
log_ << "\n";
|
|
}
|
|
|
|
void OdmOrthoPhoto::printHelp()
|
|
{
|
|
log_.setIsPrintingInCout(true);
|
|
|
|
log_ << "odm_orthophoto\n\n";
|
|
|
|
log_ << "Purpose\n";
|
|
log_ << "Create an orthograpical photo from an oriented textured mesh.\n\n";
|
|
|
|
log_ << "Usage:\n";
|
|
log_ << "The program requires a path to an input OBJ mesh file and a resolution, as pixels/m. All other input parameters are optional.\n\n";
|
|
|
|
log_ << "The following flags are available\n";
|
|
log_ << "Call the program with flag \"-help\", or without parameters to print this message, or check any generated log file.\n";
|
|
log_ << "Call the program with flag \"-verbose\", to print log messages in the standard output stream as well as in the log file.\n\n";
|
|
|
|
log_ << "Parameters are specified as: \"-<argument name> <argument>\", (without <>), and the following parameters are configureable:\n";
|
|
log_ << "\"-inputFile <path>\" (mandatory)\n";
|
|
log_ << "\"Input obj file that must contain a textured mesh.\n\n";
|
|
|
|
log_ << "\"-inputGeoRefFile <path>\" (optional, if specified boundary points are assumed to be given as world coordinates. If not specified, the boundary points are assumed to be local coordinates)\n";
|
|
log_ << "\"Input geograpical reference system file that describes the world position of the model's origin.\n\n";
|
|
|
|
log_ << "\"-outputFile <path>\" (optional, default: ortho.jpg)\n";
|
|
log_ << "\"Target file in which the orthophoto is saved.\n\n";
|
|
|
|
log_ << "\"-outputCornerFile <path>\" (optional)\n";
|
|
log_ << "\"Target text file for boundary corner points, written as \"xmin ymin xmax ymax\".\n\n";
|
|
|
|
log_ << "\"-resolution <pixels/m>\" (mandatory)\n";
|
|
log_ << "\"The number of pixels used per meter.\n\n";
|
|
|
|
log_ << "\"-boundary <Point1x Point1y Point2x Point2y Point3x Point3y Point4x Point4y>\" (optional, if not specified the entire model will be rendered)\n";
|
|
log_ << "\"Describes the area which should be covered in the ortho photo. The area will be a bounding box containing all four points. The points should be given in the same georeference system as the model.\n\n";
|
|
|
|
log_ << "\"-boundaryMinMax <MinX MinY MaxX MaxY>\" (optional, if not specified the entire model will be rendered.)\n";
|
|
log_ << "\"Describes the area which should be covered in the ortho photo. The area will be a bounding box with corners at MinX, MinY and MaxX, MaxY. The points should be given in the same georeference system as the model.\n\n";
|
|
|
|
log_.setIsPrintingInCout(false);
|
|
}
|
|
|
|
void OdmOrthoPhoto::saveTIFF(const std::string &filename, GDALDataType dataType){
|
|
GDALAllRegister();
|
|
GDALDriverH hDriver = GDALGetDriverByName( "GTiff" );
|
|
if (!hDriver){
|
|
std::cerr << "Cannot initialize GeoTIFF driver. Check your GDAL installation." << std::endl;
|
|
exit(1);
|
|
}
|
|
char **papszOptions = NULL;
|
|
GDALDatasetH hDstDS = GDALCreate( hDriver, filename.c_str(), width, height, static_cast<int>(bands.size()), dataType, papszOptions );
|
|
GDALRasterBandH hBand;
|
|
|
|
for (size_t i = 0; i < bands.size(); i++){
|
|
hBand = GDALGetRasterBand( hDstDS, static_cast<int>(i) + 1 );
|
|
if (GDALRasterIO( hBand, GF_Write, 0, 0, width, height,
|
|
bands[i], width, height, dataType, 0, 0 ) != CE_None){
|
|
std::cerr << "Cannot write TIFF to " << filename << std::endl;
|
|
exit(1);
|
|
}
|
|
// for (int j = 0; j < height; j++){
|
|
// GDALRasterIO( hBand, GF_Write, 0, j, width, 1,
|
|
// bands[i][j], width, 1, dataType, 0, 0 );
|
|
// }
|
|
}
|
|
GDALClose( hDstDS );
|
|
}
|
|
|
|
template <typename T>
|
|
inline T maxRange(){
|
|
return static_cast<T>(pow(2, sizeof(T) * 8) - 1);
|
|
}
|
|
|
|
template <typename T>
|
|
void OdmOrthoPhoto::initBands(const cv::Mat &texture){
|
|
// Channels + alpha
|
|
for (int i = 0; i < texture.channels() + 1; i++){
|
|
size_t pixelCount = static_cast<size_t>(width * height);
|
|
T *arr = new T[pixelCount];
|
|
for (size_t j = 0; j < pixelCount; j++){
|
|
arr[j] = i == 4 ? 0 : maxRange<T>(); // Set alpha at 0
|
|
}
|
|
bands.push_back(static_cast<void *>(arr));
|
|
}
|
|
}
|
|
|
|
void OdmOrthoPhoto::createOrthoPhoto()
|
|
{
|
|
if(inputFile_.empty())
|
|
{
|
|
throw OdmOrthoPhotoException("Failed to create ortho photo, no texture mesh given.");
|
|
}
|
|
|
|
if(boundaryDefined_)
|
|
{
|
|
if(inputGeoRefFile_.empty())
|
|
{
|
|
// Points are assumed to be given in as local points.
|
|
adjustBoundsForLocal();
|
|
}
|
|
else
|
|
{
|
|
// Points are assumed to be given in as world points.
|
|
adjustBoundsForGeoRef();
|
|
}
|
|
}
|
|
else if(!inputGeoRefFile_.empty())
|
|
{
|
|
// No boundary points specified, but georeference system file was given.
|
|
log_ << "Warning:\n";
|
|
log_ << "\tSpecified -inputGeoRefFile, but no boundary points. The georeference system will be ignored.\n";
|
|
}
|
|
|
|
log_ << "Reading mesh file... " << inputFile_ << "\n";
|
|
// The textured mesh.
|
|
pcl::TextureMesh mesh;
|
|
loadObjFile(inputFile_, mesh);
|
|
log_ << ".. mesh file read.\n\n";
|
|
|
|
// Does the model have more than one material?
|
|
multiMaterial_ = 1 < mesh.tex_materials.size();
|
|
|
|
bool splitModel = false;
|
|
|
|
if(multiMaterial_)
|
|
{
|
|
// Need to check relationship between texture coordinates and faces.
|
|
if(!isModelOk(mesh))
|
|
{
|
|
splitModel = true;
|
|
}
|
|
}
|
|
|
|
if(!boundaryDefined_)
|
|
{
|
|
// Determine boundary from model.
|
|
adjustBoundsForEntireModel(mesh);
|
|
}
|
|
|
|
// The minimum and maximum boundary values.
|
|
float xMax, xMin, yMax, yMin;
|
|
xMin = std::min(std::min(boundaryPoint1_[0], boundaryPoint2_[0]), std::min(boundaryPoint3_[0], boundaryPoint4_[0]));
|
|
xMax = std::max(std::max(boundaryPoint1_[0], boundaryPoint2_[0]), std::max(boundaryPoint3_[0], boundaryPoint4_[0]));
|
|
yMin = std::min(std::min(boundaryPoint1_[1], boundaryPoint2_[1]), std::min(boundaryPoint3_[1], boundaryPoint4_[1]));
|
|
yMax = std::max(std::max(boundaryPoint1_[1], boundaryPoint2_[1]), std::max(boundaryPoint3_[1], boundaryPoint4_[1]));
|
|
|
|
log_ << "Ortho photo bounds x : " << xMin << " -> " << xMax << '\n';
|
|
log_ << "Ortho photo bounds y : " << yMin << " -> " << yMax << '\n';
|
|
|
|
// The size of the area.
|
|
float xDiff = xMax - xMin;
|
|
float yDiff = yMax - yMin;
|
|
log_ << "Ortho photo area : " << xDiff*yDiff << "m2\n";
|
|
|
|
// The resolution necessary to fit the area with the given resolution.
|
|
height = static_cast<int>(std::ceil(resolution_*yDiff));
|
|
width = static_cast<int>(std::ceil(resolution_*xDiff));
|
|
log_ << "Ortho photo resolution, width x height : " << width << "x" << height << '\n';
|
|
|
|
// Check size of photo.
|
|
if(0 >= height*width)
|
|
{
|
|
if(0 >= height)
|
|
{
|
|
log_ << "Warning: ortho photo has zero area, height = " << height << ". Forcing height = 1.\n";
|
|
height = 1;
|
|
}
|
|
if(0 >= width)
|
|
{
|
|
log_ << "Warning: ortho photo has zero area, width = " << width << ". Forcing width = 1.\n";
|
|
width = 1;
|
|
}
|
|
log_ << "New ortho photo resolution, width x height : " << width << "x" << height << '\n';
|
|
}
|
|
|
|
// Contains the vertices of the mesh.
|
|
pcl::PointCloud<pcl::PointXYZ>::Ptr meshCloud (new pcl::PointCloud<pcl::PointXYZ>);
|
|
pcl::fromPCLPointCloud2 (mesh.cloud, *meshCloud);
|
|
|
|
// Split model and make copies of vertices and texture coordinates for all faces
|
|
if (splitModel)
|
|
{
|
|
pcl::PointCloud<pcl::PointXYZ>::Ptr meshCloudSplit (new pcl::PointCloud<pcl::PointXYZ>);
|
|
std::vector<Eigen::Vector2f, Eigen::aligned_allocator<Eigen::Vector2f> > textureCoordinates = std::vector<Eigen::Vector2f, Eigen::aligned_allocator<Eigen::Vector2f> >(0);
|
|
|
|
size_t vertexIndexCount = 0;
|
|
for(size_t t = 0; t < mesh.tex_polygons.size(); ++t)
|
|
{
|
|
vertexIndexCount += 3 * mesh.tex_polygons[t].size();
|
|
}
|
|
textureCoordinates.reserve(vertexIndexCount);
|
|
|
|
for(size_t t = 0; t < mesh.tex_polygons.size(); ++t)
|
|
{
|
|
|
|
for(size_t faceIndex = 0; faceIndex < mesh.tex_polygons[t].size(); ++faceIndex)
|
|
{
|
|
pcl::Vertices polygon = mesh.tex_polygons[t][faceIndex];
|
|
|
|
// The index to the vertices of the polygon.
|
|
size_t v1i = polygon.vertices[0];
|
|
size_t v2i = polygon.vertices[1];
|
|
size_t v3i = polygon.vertices[2];
|
|
|
|
// The polygon's points.
|
|
pcl::PointXYZ v1 = meshCloud->points[v1i];
|
|
pcl::PointXYZ v2 = meshCloud->points[v2i];
|
|
pcl::PointXYZ v3 = meshCloud->points[v3i];
|
|
|
|
Eigen::Vector2f vt1 = mesh.tex_coordinates[0][3*faceIndex];
|
|
Eigen::Vector2f vt2 = mesh.tex_coordinates[0][3*faceIndex + 1];
|
|
Eigen::Vector2f vt3 = mesh.tex_coordinates[0][3*faceIndex + 2];
|
|
|
|
meshCloudSplit->points.push_back(v1);
|
|
textureCoordinates.push_back(vt1);
|
|
mesh.tex_polygons[t][faceIndex].vertices[0] = vertexIndexCount;
|
|
|
|
meshCloudSplit->points.push_back(v2);
|
|
textureCoordinates.push_back(vt2);
|
|
mesh.tex_polygons[t][faceIndex].vertices[1] = vertexIndexCount;
|
|
|
|
meshCloudSplit->points.push_back(v3);
|
|
textureCoordinates.push_back(vt3);
|
|
mesh.tex_polygons[t][faceIndex].vertices[2] = vertexIndexCount;
|
|
}
|
|
}
|
|
|
|
mesh.tex_coordinates.clear();
|
|
mesh.tex_coordinates.push_back(textureCoordinates);
|
|
|
|
meshCloud = meshCloudSplit;
|
|
}
|
|
|
|
// Creates a transformation which aligns the area for the ortho photo.
|
|
Eigen::Transform<float, 3, Eigen::Affine> transform = getROITransform(xMin, -yMax);
|
|
|
|
log_ << "Translating and scaling mesh...\n";
|
|
|
|
// Move the mesh into position.
|
|
pcl::transformPointCloud(*meshCloud, *meshCloud, transform);
|
|
log_ << ".. mesh translated and scaled.\n\n";
|
|
|
|
// Flatten texture coordinates.
|
|
std::vector<Eigen::Vector2f> uvs;
|
|
uvs.reserve(mesh.tex_coordinates.size());
|
|
for(size_t t = 0; t < mesh.tex_coordinates.size(); ++t)
|
|
{
|
|
uvs.insert(uvs.end(), mesh.tex_coordinates[t].begin(), mesh.tex_coordinates[t].end());
|
|
}
|
|
//cv::namedWindow("dsfs");
|
|
|
|
// The current material texture
|
|
cv::Mat texture;
|
|
|
|
// Used to keep track of the global face index.
|
|
size_t faceOff = 0;
|
|
|
|
log_ << "Rendering the ortho photo...\n";
|
|
|
|
// Iterate over each part of the mesh (one per material).
|
|
int textureDepth = -1;
|
|
|
|
for(size_t t = 0; t < mesh.tex_materials.size(); ++t)
|
|
{
|
|
// The material of the current submesh.
|
|
pcl::TexMaterial material = mesh.tex_materials[t];
|
|
texture = cv::imread(material.tex_file, cv::IMREAD_COLOR | cv::IMREAD_ANYDEPTH);
|
|
|
|
// The first material determines the bit depth
|
|
// Init ortho photo
|
|
if (textureDepth == -1){
|
|
try{
|
|
textureDepth = texture.depth();
|
|
log_ << "Texture channels: " << texture.channels() << "\n";
|
|
|
|
if (textureDepth == CV_8U){
|
|
log_ << "Texture depth: 8bit\n";
|
|
initBands<uint8_t>(texture);
|
|
}else if (textureDepth == CV_16U){
|
|
log_ << "Texture depth: 16bit\n";
|
|
initBands<uint16_t>(texture);
|
|
}else{
|
|
std::cerr << "Unsupported bit depth value: " << textureDepth;
|
|
exit(1);
|
|
}
|
|
|
|
depth_ = cv::Mat::zeros(height, width, CV_32F) - std::numeric_limits<float>::infinity();
|
|
}catch(const cv::Exception &e){
|
|
std::cerr << "Couldn't allocate enough memory to render the orthophoto (" << width << "x" << height << " cells = " << ((long long)width * (long long)height * 4) << " bytes). Try to increase the --orthophoto-resolution parameter to a larger integer or add more RAM.\n";
|
|
exit(1);
|
|
}
|
|
}
|
|
|
|
// Check for missing files.
|
|
if(texture.empty())
|
|
{
|
|
log_ << "Material texture could not be read:\n";
|
|
log_ << material.tex_file << '\n';
|
|
log_ << "Could not be read as image, does the file exist?\n";
|
|
continue; // Skip to next material.
|
|
}
|
|
|
|
// The faces of the current submesh.
|
|
std::vector<pcl::Vertices> faces = mesh.tex_polygons[t];
|
|
|
|
// Iterate over each face...
|
|
for(size_t faceIndex = 0; faceIndex < faces.size(); ++faceIndex)
|
|
{
|
|
// The current polygon.
|
|
pcl::Vertices polygon = faces[faceIndex];
|
|
|
|
// ... and draw it into the ortho photo.
|
|
if (textureDepth == CV_8U){
|
|
drawTexturedTriangle<uint8_t>(texture, polygon, meshCloud, uvs, faceIndex+faceOff);
|
|
}else if (textureDepth == CV_16U){
|
|
drawTexturedTriangle<uint16_t>(texture, polygon, meshCloud, uvs, faceIndex+faceOff);
|
|
}
|
|
}
|
|
faceOff += faces.size();
|
|
log_ << "Material " << t << " rendered.\n";
|
|
}
|
|
log_ << "...ortho photo rendered\n";
|
|
|
|
log_ << '\n';
|
|
log_ << "Writing ortho photo to " << outputFile_ << "\n";
|
|
|
|
if (textureDepth == CV_8U){
|
|
saveTIFF(outputFile_, GDT_Byte);
|
|
}else if (textureDepth == CV_16U){
|
|
saveTIFF(outputFile_, GDT_UInt16);
|
|
}else{
|
|
std::cerr << "Unsupported bit depth value: " << textureDepth;
|
|
exit(1);
|
|
}
|
|
|
|
// cv::imwrite(outputFile_, photo_);
|
|
|
|
if (!outputCornerFile_.empty())
|
|
{
|
|
log_ << "Writing corner coordinates to " << outputCornerFile_ << "\n";
|
|
std::ofstream cornerStream(outputCornerFile_.c_str());
|
|
if (!cornerStream.is_open())
|
|
{
|
|
throw OdmOrthoPhotoException("Failed opening output corner file " + outputCornerFile_ + ".");
|
|
}
|
|
cornerStream.setf(std::ios::scientific, std::ios::floatfield);
|
|
cornerStream.precision(17);
|
|
cornerStream << xMin << " " << yMin << " " << xMax << " " << yMax;
|
|
cornerStream.close();
|
|
}
|
|
|
|
log_ << "Orthophoto generation done.\n";
|
|
}
|
|
|
|
void OdmOrthoPhoto::adjustBoundsForGeoRef()
|
|
{
|
|
log_ << "Adjusting bounds for world coordinates\n";
|
|
|
|
// A stream of the georef system.
|
|
std::ifstream geoRefStream(inputGeoRefFile_.c_str());
|
|
|
|
// The system name
|
|
std::string system;
|
|
// The east and north offsets
|
|
int eastOffset, northOffset;
|
|
|
|
// Parse file
|
|
std::getline(geoRefStream, system);
|
|
if(!(geoRefStream >> eastOffset))
|
|
{
|
|
throw OdmOrthoPhotoException("Could not extract geographical reference system from \n" + inputGeoRefFile_ + "\nCould not extract east offset.");
|
|
}
|
|
if(!(geoRefStream >> northOffset))
|
|
{
|
|
throw OdmOrthoPhotoException("Could not extract geographical reference system from \n" + inputGeoRefFile_ + "\nCould not extract north offset.");
|
|
}
|
|
|
|
log_ << "Georeference system:\n";
|
|
log_ << system << "\n";
|
|
log_ << "East offset: " << eastOffset << "\n";
|
|
log_ << "North offset: " << northOffset << "\n";
|
|
|
|
// Adjust boundary points.
|
|
boundaryPoint1_[0] = static_cast<float>(worldPoint1_.eastInteger_ - eastOffset) + worldPoint1_.eastFractional_;
|
|
boundaryPoint1_[1] = static_cast<float>(worldPoint1_.northInteger_ - northOffset) + worldPoint1_.northFractional_;
|
|
boundaryPoint2_[0] = static_cast<float>(worldPoint2_.eastInteger_ - eastOffset) + worldPoint2_.eastFractional_;
|
|
boundaryPoint2_[1] = static_cast<float>(worldPoint2_.northInteger_ - northOffset) + worldPoint2_.northFractional_;
|
|
boundaryPoint3_[0] = static_cast<float>(worldPoint3_.eastInteger_ - eastOffset) + worldPoint3_.eastFractional_;
|
|
boundaryPoint3_[1] = static_cast<float>(worldPoint3_.northInteger_ - northOffset) + worldPoint3_.northFractional_;
|
|
boundaryPoint4_[0] = static_cast<float>(worldPoint4_.eastInteger_ - eastOffset) + worldPoint4_.eastFractional_;
|
|
boundaryPoint4_[1] = static_cast<float>(worldPoint4_.northInteger_ - northOffset) + worldPoint4_.northFractional_;
|
|
|
|
log_ << "Local boundary points:\n";
|
|
log_ << "Point 1: " << boundaryPoint1_[0] << " " << boundaryPoint1_[1] << "\n";
|
|
log_ << "Point 2: " << boundaryPoint2_[0] << " " << boundaryPoint2_[1] << "\n";
|
|
log_ << "Point 3: " << boundaryPoint3_[0] << " " << boundaryPoint3_[1] << "\n";
|
|
log_ << "Point 4: " << boundaryPoint4_[0] << " " << boundaryPoint4_[1] << "\n";
|
|
}
|
|
|
|
void OdmOrthoPhoto::adjustBoundsForLocal()
|
|
{
|
|
log_ << "Adjusting bounds for local coordinates\n";
|
|
|
|
// Set boundary points from world points.
|
|
boundaryPoint1_[0] = static_cast<float>(worldPoint1_.eastInteger_ ) + worldPoint1_.eastFractional_;
|
|
boundaryPoint1_[1] = static_cast<float>(worldPoint1_.northInteger_) + worldPoint1_.northFractional_;
|
|
boundaryPoint2_[0] = static_cast<float>(worldPoint2_.eastInteger_ ) + worldPoint2_.eastFractional_;
|
|
boundaryPoint2_[1] = static_cast<float>(worldPoint2_.northInteger_) + worldPoint2_.northFractional_;
|
|
boundaryPoint3_[0] = static_cast<float>(worldPoint3_.eastInteger_ ) + worldPoint3_.eastFractional_;
|
|
boundaryPoint3_[1] = static_cast<float>(worldPoint3_.northInteger_) + worldPoint3_.northFractional_;
|
|
boundaryPoint4_[0] = static_cast<float>(worldPoint4_.eastInteger_ ) + worldPoint4_.eastFractional_;
|
|
boundaryPoint4_[1] = static_cast<float>(worldPoint4_.northInteger_) + worldPoint4_.northFractional_;
|
|
|
|
log_ << "Local boundary points:\n";
|
|
log_ << "Point 1: " << boundaryPoint1_[0] << " " << boundaryPoint1_[1] << "\n";
|
|
log_ << "Point 2: " << boundaryPoint2_[0] << " " << boundaryPoint2_[1] << "\n";
|
|
log_ << "Point 3: " << boundaryPoint3_[0] << " " << boundaryPoint3_[1] << "\n";
|
|
log_ << "Point 4: " << boundaryPoint4_[0] << " " << boundaryPoint4_[1] << "\n";
|
|
log_ << "\n";
|
|
}
|
|
|
|
void OdmOrthoPhoto::adjustBoundsForEntireModel(const pcl::TextureMesh &mesh)
|
|
{
|
|
log_ << "Set boundary to contain entire model.\n";
|
|
|
|
// The boundary of the model.
|
|
float xMin, xMax, yMin, yMax;
|
|
|
|
xMin = std::numeric_limits<float>::infinity();
|
|
xMax = -std::numeric_limits<float>::infinity();
|
|
yMin = std::numeric_limits<float>::infinity();
|
|
yMax = -std::numeric_limits<float>::infinity();
|
|
|
|
// Contains the vertices of the mesh.
|
|
pcl::PointCloud<pcl::PointXYZ>::Ptr meshCloud (new pcl::PointCloud<pcl::PointXYZ>);
|
|
pcl::fromPCLPointCloud2 (mesh.cloud, *meshCloud);
|
|
|
|
for(size_t t = 0; t < mesh.tex_materials.size(); ++t)
|
|
{
|
|
// The faces of the current submesh.
|
|
std::vector<pcl::Vertices> faces = mesh.tex_polygons[t];
|
|
|
|
// Iterate over each face...
|
|
for(size_t faceIndex = 0; faceIndex < faces.size(); ++faceIndex)
|
|
{
|
|
// The current polygon.
|
|
pcl::Vertices polygon = faces[faceIndex];
|
|
|
|
// The index to the vertices of the polygon.
|
|
size_t v1i = polygon.vertices[0];
|
|
size_t v2i = polygon.vertices[1];
|
|
size_t v3i = polygon.vertices[2];
|
|
|
|
// The polygon's points.
|
|
pcl::PointXYZ v1 = meshCloud->points[v1i];
|
|
pcl::PointXYZ v2 = meshCloud->points[v2i];
|
|
pcl::PointXYZ v3 = meshCloud->points[v3i];
|
|
|
|
xMin = std::min(std::min(xMin, v1.x), std::min(v2.x, v3.x));
|
|
xMax = std::max(std::max(xMax, v1.x), std::max(v2.x, v3.x));
|
|
yMin = std::min(std::min(yMin, v1.y), std::min(v2.y, v3.y));
|
|
yMax = std::max(std::max(yMax, v1.y), std::max(v2.y, v3.y));
|
|
}
|
|
}
|
|
|
|
// Create dummy boundary points.
|
|
boundaryPoint1_[0] = xMin; boundaryPoint1_[1] = yMin;
|
|
boundaryPoint2_[0] = xMin; boundaryPoint2_[1] = yMax;
|
|
boundaryPoint3_[0] = xMax; boundaryPoint3_[1] = yMax;
|
|
boundaryPoint4_[0] = xMax; boundaryPoint4_[1] = yMin;
|
|
|
|
log_ << "Local boundary points:\n";
|
|
log_ << "Point 1: " << boundaryPoint1_[0] << " " << boundaryPoint1_[1] << "\n";
|
|
log_ << "Point 2: " << boundaryPoint2_[0] << " " << boundaryPoint2_[1] << "\n";
|
|
log_ << "Point 3: " << boundaryPoint3_[0] << " " << boundaryPoint3_[1] << "\n";
|
|
log_ << "Point 4: " << boundaryPoint4_[0] << " " << boundaryPoint4_[1] << "\n";
|
|
log_ << "\n";
|
|
}
|
|
|
|
Eigen::Transform<float, 3, Eigen::Affine> OdmOrthoPhoto::getROITransform(float xMin, float yMin) const
|
|
{
|
|
//Use transformation matrix if provided:
|
|
if(transformOverride_){
|
|
return readTransform(inputTransformFile_);
|
|
}
|
|
else {
|
|
// The transform used to move the chosen area into the ortho photo.
|
|
Eigen::Transform<float, 3, Eigen::Affine> transform;
|
|
|
|
transform(0, 0) = resolution_; // x Scaling.
|
|
transform(1, 0) = 0.0f;
|
|
transform(2, 0) = 0.0f;
|
|
transform(3, 0) = 0.0f;
|
|
|
|
transform(0, 1) = 0.0f;
|
|
transform(1, 1) = -resolution_; // y Scaling, mirrored for easier rendering.
|
|
transform(2, 1) = 0.0f;
|
|
transform(3, 1) = 0.0f;
|
|
|
|
transform(0, 2) = 0.0f;
|
|
transform(1, 2) = 0.0f;
|
|
transform(2, 2) = 1.0f;
|
|
transform(3, 2) = 0.0f;
|
|
|
|
transform(0, 3) = -xMin * resolution_; // x Translation
|
|
transform(1, 3) = -yMin * resolution_; // y Translation
|
|
transform(2, 3) = 0.0f;
|
|
transform(3, 3) = 1.0f;
|
|
|
|
return transform;
|
|
}
|
|
}
|
|
|
|
Eigen::Transform<float, 3, Eigen::Affine> OdmOrthoPhoto::readTransform(std::string transformFile_) const
|
|
{
|
|
Eigen::Transform<float, 3, Eigen::Affine> transform;
|
|
|
|
std::ifstream transStream(transformFile_.c_str());
|
|
if (!transStream.good())
|
|
{
|
|
throw OdmOrthoPhotoException("Failed opening coordinate file " + transformFile_ + " for reading. " + '\n');
|
|
}
|
|
|
|
std::string transString;
|
|
{
|
|
std::getline(transStream, transString);
|
|
std::stringstream l1(transString);
|
|
l1 >> transform(0,0) >> transform(0,1) >> transform(0,2) >> transform(0,3);
|
|
|
|
std::getline(transStream, transString);
|
|
std::stringstream l2(transString);
|
|
l2 >> transform(1,0) >> transform(1,1) >> transform(1,2) >> transform(1,3);
|
|
|
|
std::getline(transStream, transString);
|
|
std::stringstream l3(transString);
|
|
l3 >> transform(2,0) >> transform(2,1) >> transform(2,2) >> transform(2,3);
|
|
|
|
std::getline(transStream, transString);
|
|
std::stringstream l4(transString);
|
|
l4 >> transform(3,0) >> transform(3,1) >> transform(3,2) >> transform(3,3);
|
|
}
|
|
|
|
// Don't do any rotation/shear
|
|
transform(0,1) = 0.0f;
|
|
transform(1,0) = 0.0f;
|
|
|
|
return transform;
|
|
}
|
|
|
|
template <typename T>
|
|
void OdmOrthoPhoto::drawTexturedTriangle(const cv::Mat &texture, const pcl::Vertices &polygon, const pcl::PointCloud<pcl::PointXYZ>::Ptr &meshCloud, const std::vector<Eigen::Vector2f> &uvs, size_t faceIndex)
|
|
{
|
|
// The index to the vertices of the polygon.
|
|
size_t v1i = polygon.vertices[0];
|
|
size_t v2i = polygon.vertices[1];
|
|
size_t v3i = polygon.vertices[2];
|
|
|
|
// The polygon's points.
|
|
pcl::PointXYZ v1 = meshCloud->points[v1i];
|
|
pcl::PointXYZ v2 = meshCloud->points[v2i];
|
|
pcl::PointXYZ v3 = meshCloud->points[v3i];
|
|
|
|
if(isSliverPolygon(v1, v2, v3))
|
|
{
|
|
log_ << "Warning: Sliver polygon found at face index " << faceIndex << '\n';
|
|
return;
|
|
}
|
|
|
|
// The face data. Position v*{x,y,z}. Texture coordinate v*{u,v}. * is the vertex number in the polygon.
|
|
float v1x, v1y, v1z, v1u, v1v;
|
|
float v2x, v2y, v2z, v2u, v2v;
|
|
float v3x, v3y, v3z, v3u, v3v;
|
|
|
|
// Barycentric coordinates of the currently rendered point.
|
|
float l1, l2, l3;
|
|
|
|
// The size of the photo, as float.
|
|
float fRows, fCols;
|
|
fRows = static_cast<float>(texture.rows);
|
|
fCols = static_cast<float>(texture.cols);
|
|
|
|
// Get vertex position.
|
|
v1x = v1.x; v1y = v1.y; v1z = v1.z;
|
|
v2x = v2.x; v2y = v2.y; v2z = v2.z;
|
|
v3x = v3.x; v3y = v3.y; v3z = v3.z;
|
|
|
|
// Get texture coordinates.
|
|
v1u = uvs[3*faceIndex][0]; v1v = uvs[3*faceIndex][1];
|
|
v2u = uvs[3*faceIndex+1][0]; v2v = uvs[3*faceIndex+1][1];
|
|
v3u = uvs[3*faceIndex+2][0]; v3v = uvs[3*faceIndex+2][1];
|
|
|
|
// Check bounding box overlap.
|
|
int xMin = static_cast<int>(std::min(std::min(v1x, v2x), v3x));
|
|
if(xMin > width)
|
|
{
|
|
return; // Completely outside to the right.
|
|
}
|
|
int xMax = static_cast<int>(std::max(std::max(v1x, v2x), v3x));
|
|
if(xMax < 0)
|
|
{
|
|
return; // Completely outside to the left.
|
|
}
|
|
int yMin = static_cast<int>(std::min(std::min(v1y, v2y), v3y));
|
|
if(yMin > height)
|
|
{
|
|
return; // Completely outside to the top.
|
|
}
|
|
int yMax = static_cast<int>(std::max(std::max(v1y, v2y), v3y));
|
|
if(yMax < 0)
|
|
{
|
|
return; // Completely outside to the bottom.
|
|
}
|
|
|
|
// Top point row and column positions
|
|
float topR, topC;
|
|
// Middle point row and column positions
|
|
float midR, midC;
|
|
// Bottom point row and column positions
|
|
float botR, botC;
|
|
|
|
// Find top, middle and bottom points.
|
|
if(v1y < v2y)
|
|
{
|
|
if(v1y < v3y)
|
|
{
|
|
if(v2y < v3y)
|
|
{
|
|
// 1 -> 2 -> 3
|
|
topR = v1y; topC = v1x;
|
|
midR = v2y; midC = v2x;
|
|
botR = v3y; botC = v3x;
|
|
}
|
|
else
|
|
{
|
|
// 1 -> 3 -> 2
|
|
topR = v1y; topC = v1x;
|
|
midR = v3y; midC = v3x;
|
|
botR = v2y; botC = v2x;
|
|
}
|
|
}
|
|
else
|
|
{
|
|
// 3 -> 1 -> 2
|
|
topR = v3y; topC = v3x;
|
|
midR = v1y; midC = v1x;
|
|
botR = v2y; botC = v2x;
|
|
}
|
|
}
|
|
else // v2y <= v1y
|
|
{
|
|
if(v2y < v3y)
|
|
{
|
|
if(v1y < v3y)
|
|
{
|
|
// 2 -> 1 -> 3
|
|
topR = v2y; topC = v2x;
|
|
midR = v1y; midC = v1x;
|
|
botR = v3y; botC = v3x;
|
|
}
|
|
else
|
|
{
|
|
// 2 -> 3 -> 1
|
|
topR = v2y; topC = v2x;
|
|
midR = v3y; midC = v3x;
|
|
botR = v1y; botC = v1x;
|
|
}
|
|
}
|
|
else
|
|
{
|
|
// 3 -> 2 -> 1
|
|
topR = v3y; topC = v3x;
|
|
midR = v2y; midC = v2x;
|
|
botR = v1y; botC = v1x;
|
|
}
|
|
}
|
|
|
|
// General appreviations:
|
|
// ---------------------
|
|
// tm : Top(to)Middle.
|
|
// mb : Middle(to)Bottom.
|
|
// tb : Top(to)Bottom.
|
|
// c : column.
|
|
// r : row.
|
|
// dr : DeltaRow, step value per row.
|
|
|
|
// The step along column for every step along r. Top to middle.
|
|
float ctmdr;
|
|
// The step along column for every step along r. Top to bottom.
|
|
float ctbdr;
|
|
// The step along column for every step along r. Middle to bottom.
|
|
float cmbdr;
|
|
|
|
ctbdr = (botC-topC)/(botR-topR);
|
|
|
|
// The current column position, from top to middle.
|
|
float ctm = topC;
|
|
// The current column position, from top to bottom.
|
|
float ctb = topC;
|
|
|
|
// Check for vertical line between middle and top.
|
|
if(FLT_EPSILON < midR-topR)
|
|
{
|
|
ctmdr = (midC-topC)/(midR-topR);
|
|
|
|
// The first pixel row for the bottom part of the triangle.
|
|
int rqStart = std::max(static_cast<int>(std::floor(topR+0.5f)), 0);
|
|
// The last pixel row for the top part of the triangle.
|
|
int rqEnd = std::min(static_cast<int>(std::floor(midR+0.5f)), height);
|
|
|
|
// Traverse along row from top to middle.
|
|
for(int rq = rqStart; rq < rqEnd; ++rq)
|
|
{
|
|
// Set the current column positions.
|
|
ctm = topC + ctmdr*(static_cast<float>(rq)+0.5f-topR);
|
|
ctb = topC + ctbdr*(static_cast<float>(rq)+0.5f-topR);
|
|
|
|
// The first pixel column for the current row.
|
|
int cqStart = std::max(static_cast<int>(std::floor(0.5f+std::min(ctm, ctb))), 0);
|
|
// The last pixel column for the current row.
|
|
int cqEnd = std::min(static_cast<int>(std::floor(0.5f+std::max(ctm, ctb))), width);
|
|
|
|
for(int cq = cqStart; cq < cqEnd; ++cq)
|
|
{
|
|
// Get barycentric coordinates for the current point.
|
|
getBarycentricCoordinates(v1, v2, v3, static_cast<float>(cq)+0.5f, static_cast<float>(rq)+0.5f, l1, l2, l3);
|
|
|
|
if(0.f > l1 || 0.f > l2 || 0.f > l3)
|
|
{
|
|
//continue;
|
|
}
|
|
|
|
// The z value for the point.
|
|
float z = v1z*l1+v2z*l2+v3z*l3;
|
|
|
|
// Check depth
|
|
float depthValue = depth_.at<float>(rq, cq);
|
|
if(z < depthValue)
|
|
{
|
|
// Current is behind another, don't draw.
|
|
continue;
|
|
}
|
|
|
|
// The uv values of the point.
|
|
float u, v;
|
|
u = v1u*l1+v2u*l2+v3u*l3;
|
|
v = v1v*l1+v2v*l2+v3v*l3;
|
|
|
|
renderPixel<T>(rq, cq, u*fCols, (1.0f-v)*fRows, texture);
|
|
|
|
// Update depth buffer.
|
|
depth_.at<float>(rq, cq) = z;
|
|
}
|
|
}
|
|
}
|
|
|
|
if(FLT_EPSILON < botR-midR)
|
|
{
|
|
cmbdr = (botC-midC)/(botR-midR);
|
|
|
|
// The current column position, from middle to bottom.
|
|
float cmb = midC;
|
|
|
|
// The first pixel row for the bottom part of the triangle.
|
|
int rqStart = std::max(static_cast<int>(std::floor(midR+0.5f)), 0);
|
|
// The last pixel row for the bottom part of the triangle.
|
|
int rqEnd = std::min(static_cast<int>(std::floor(botR+0.5f)), height);
|
|
|
|
// Traverse along row from middle to bottom.
|
|
for(int rq = rqStart; rq < rqEnd; ++rq)
|
|
{
|
|
// Set the current column positions.
|
|
ctb = topC + ctbdr*(static_cast<float>(rq)+0.5f-topR);
|
|
cmb = midC + cmbdr*(static_cast<float>(rq)+0.5f-midR);
|
|
|
|
// The first pixel column for the current row.
|
|
int cqStart = std::max(static_cast<int>(std::floor(0.5f+std::min(cmb, ctb))), 0);
|
|
// The last pixel column for the current row.
|
|
int cqEnd = std::min(static_cast<int>(std::floor(0.5f+std::max(cmb, ctb))), width);
|
|
|
|
for(int cq = cqStart; cq < cqEnd; ++cq)
|
|
{
|
|
// Get barycentric coordinates for the current point.
|
|
getBarycentricCoordinates(v1, v2, v3, static_cast<float>(cq)+0.5f, static_cast<float>(rq)+0.5f, l1, l2, l3);
|
|
|
|
if(0.f > l1 || 0.f > l2 || 0.f > l3)
|
|
{
|
|
//continue;
|
|
}
|
|
|
|
// The z value for the point.
|
|
float z = v1z*l1+v2z*l2+v3z*l3;
|
|
|
|
// Check depth
|
|
float depthValue = depth_.at<float>(rq, cq);
|
|
if(z < depthValue)
|
|
{
|
|
// Current is behind another, don't draw.
|
|
continue;
|
|
}
|
|
|
|
// The uv values of the point.
|
|
float u, v;
|
|
u = v1u*l1+v2u*l2+v3u*l3;
|
|
v = v1v*l1+v2v*l2+v3v*l3;
|
|
|
|
renderPixel<T>(rq, cq, u*fCols, (1.0f-v)*fRows, texture);
|
|
|
|
// Update depth buffer.
|
|
depth_.at<float>(rq, cq) = z;
|
|
}
|
|
}
|
|
}
|
|
}
|
|
|
|
template <typename T>
|
|
void OdmOrthoPhoto::renderPixel(int row, int col, float s, float t, const cv::Mat &texture)
|
|
{
|
|
// The colors of the texture pixels. tl : top left, tr : top right, bl : bottom left, br : bottom right.
|
|
cv::Vec<T,3> tl, tr, bl, br;
|
|
|
|
// The offset of the texture coordinate from its pixel positions.
|
|
float leftF, topF;
|
|
// The position of the top left pixel.
|
|
int left, top;
|
|
// The distance to the left and right pixel from the texture coordinate.
|
|
float dl, dt;
|
|
// The distance to the top and bottom pixel from the texture coordinate.
|
|
float dr, db;
|
|
|
|
dl = modff(s, &leftF);
|
|
dr = 1.0f - dl;
|
|
dt = modff(t, &topF);
|
|
db = 1.0f - dt;
|
|
|
|
left = static_cast<int>(leftF);
|
|
top = static_cast<int>(topF);
|
|
|
|
tl = texture.at<cv::Vec<T,3> >(top, left);
|
|
tr = texture.at<cv::Vec<T,3> >(top, left+1);
|
|
bl = texture.at<cv::Vec<T,3> >(top+1, left);
|
|
br = texture.at<cv::Vec<T,3> >(top+1, left+1);
|
|
|
|
// The interpolated color values.
|
|
float r = 0.0f, g = 0.0f, b = 0.0f;
|
|
|
|
// Red
|
|
r += static_cast<float>(tl[2]) * dr * db;
|
|
r += static_cast<float>(tr[2]) * dl * db;
|
|
r += static_cast<float>(bl[2]) * dr * dt;
|
|
r += static_cast<float>(br[2]) * dl * dt;
|
|
|
|
// Green
|
|
g += static_cast<float>(tl[1]) * dr * db;
|
|
g += static_cast<float>(tr[1]) * dl * db;
|
|
g += static_cast<float>(bl[1]) * dr * dt;
|
|
g += static_cast<float>(br[1]) * dl * dt;
|
|
|
|
// Blue
|
|
b += static_cast<float>(tl[0]) * dr * db;
|
|
b += static_cast<float>(tr[0]) * dl * db;
|
|
b += static_cast<float>(bl[0]) * dr * dt;
|
|
b += static_cast<float>(br[0]) * dl * dt;
|
|
|
|
size_t idx = static_cast<size_t>(row * width + col);
|
|
static_cast<T *>(bands[0])[idx] = static_cast<T>(r);
|
|
static_cast<T *>(bands[1])[idx] = static_cast<T>(g);
|
|
static_cast<T *>(bands[2])[idx] = static_cast<T>(b);
|
|
static_cast<T *>(bands[3])[idx] = static_cast<T>(255); // Alpha should always be in the 255 range
|
|
}
|
|
|
|
void OdmOrthoPhoto::getBarycentricCoordinates(pcl::PointXYZ v1, pcl::PointXYZ v2, pcl::PointXYZ v3, float x, float y, float &l1, float &l2, float &l3) const
|
|
{
|
|
// Diff along y.
|
|
float y2y3 = v2.y-v3.y;
|
|
float y1y3 = v1.y-v3.y;
|
|
float y3y1 = v3.y-v1.y;
|
|
float yy3 = y -v3.y;
|
|
|
|
// Diff along x.
|
|
float x3x2 = v3.x-v2.x;
|
|
float x1x3 = v1.x-v3.x;
|
|
float xx3 = x -v3.x;
|
|
|
|
// Normalization factor.
|
|
float norm = (y2y3*x1x3 + x3x2*y1y3);
|
|
|
|
l1 = (y2y3*(xx3) + x3x2*(yy3)) / norm;
|
|
l2 = (y3y1*(xx3) + x1x3*(yy3)) / norm;
|
|
l3 = 1 - l1 - l2;
|
|
}
|
|
|
|
bool OdmOrthoPhoto::isSliverPolygon(pcl::PointXYZ v1, pcl::PointXYZ v2, pcl::PointXYZ v3) const
|
|
{
|
|
// Calculations are made using doubles, to minize rounding errors.
|
|
Eigen::Vector3d a = Eigen::Vector3d(static_cast<double>(v1.x), static_cast<double>(v1.y), static_cast<double>(v1.z));
|
|
Eigen::Vector3d b = Eigen::Vector3d(static_cast<double>(v2.x), static_cast<double>(v2.y), static_cast<double>(v2.z));
|
|
Eigen::Vector3d c = Eigen::Vector3d(static_cast<double>(v3.x), static_cast<double>(v3.y), static_cast<double>(v3.z));
|
|
Eigen::Vector3d dummyVec = (a-b).cross(c-b);
|
|
|
|
// Area smaller than, or equal to, floating-point epsilon.
|
|
return std::numeric_limits<float>::epsilon() >= static_cast<float>(std::sqrt(dummyVec.dot(dummyVec))/2.0);
|
|
}
|
|
|
|
bool OdmOrthoPhoto::isModelOk(const pcl::TextureMesh &mesh)
|
|
{
|
|
// The number of texture coordinates in the model.
|
|
size_t nTextureCoordinates = 0;
|
|
// The number of faces in the model.
|
|
size_t nFaces = 0;
|
|
|
|
for(size_t t = 0; t < mesh.tex_coordinates.size(); ++t)
|
|
{
|
|
nTextureCoordinates += mesh.tex_coordinates[t].size();
|
|
}
|
|
for(size_t t = 0; t < mesh.tex_polygons.size(); ++t)
|
|
{
|
|
nFaces += mesh.tex_polygons[t].size();
|
|
}
|
|
|
|
log_ << "Number of faces in the model " << nFaces << '\n';
|
|
|
|
return 3*nFaces == nTextureCoordinates;
|
|
}
|
|
|
|
|
|
bool OdmOrthoPhoto::loadObjFile(std::string inputFile, pcl::TextureMesh &mesh)
|
|
{
|
|
int data_type;
|
|
unsigned int data_idx;
|
|
int file_version;
|
|
int offset = 0;
|
|
Eigen::Vector4f origin;
|
|
Eigen::Quaternionf orientation;
|
|
|
|
if (!readHeader(inputFile, mesh.cloud, origin, orientation, file_version, data_type, data_idx, offset))
|
|
{
|
|
throw OdmOrthoPhotoException("Problem reading header in modelfile!\n");
|
|
}
|
|
|
|
std::ifstream fs;
|
|
|
|
fs.open (inputFile.c_str (), std::ios::binary);
|
|
if (!fs.is_open () || fs.fail ())
|
|
{
|
|
//PCL_ERROR ("[pcl::OBJReader::readHeader] Could not open file '%s'! Error : %s\n", file_name.c_str (), strerror(errno));
|
|
fs.close ();
|
|
log_<<"Could not read mesh from file ";
|
|
log_ << inputFile.c_str();
|
|
log_ <<"\n";
|
|
|
|
throw OdmOrthoPhotoException("Problem reading mesh from file!\n");
|
|
}
|
|
|
|
// Seek at the given offset
|
|
fs.seekg (data_idx, std::ios::beg);
|
|
|
|
// Get normal_x field indices
|
|
int normal_x_field = -1;
|
|
for (std::size_t i = 0; i < mesh.cloud.fields.size (); ++i)
|
|
{
|
|
if (mesh.cloud.fields[i].name == "normal_x")
|
|
{
|
|
normal_x_field = i;
|
|
break;
|
|
}
|
|
}
|
|
|
|
std::size_t v_idx = 0;
|
|
std::size_t vn_idx = 0;
|
|
std::size_t vt_idx = 0;
|
|
std::size_t f_idx = 0;
|
|
std::string line;
|
|
std::vector<std::string> st;
|
|
std::vector<Eigen::Vector2f, Eigen::aligned_allocator<Eigen::Vector2f> > coordinates;
|
|
std::vector<Eigen::Vector2f> allTexCoords;
|
|
|
|
std::map<int, int> f2vt;
|
|
|
|
try
|
|
{
|
|
while (!fs.eof ())
|
|
{
|
|
getline (fs, line);
|
|
// Ignore empty lines
|
|
if (line == "")
|
|
continue;
|
|
|
|
// Tokenize the line
|
|
std::stringstream sstream (line);
|
|
sstream.imbue (std::locale::classic ());
|
|
line = sstream.str ();
|
|
boost::trim (line);
|
|
boost::split (st, line, boost::is_any_of ("\t\r "), boost::token_compress_on);
|
|
|
|
// Ignore comments
|
|
if (st[0] == "#")
|
|
continue;
|
|
// Vertex
|
|
if (st[0] == "v")
|
|
{
|
|
try
|
|
{
|
|
for (int i = 1, f = 0; i < 4; ++i, ++f)
|
|
{
|
|
float value = boost::lexical_cast<float> (st[i]);
|
|
memcpy (&mesh.cloud.data[v_idx * mesh.cloud.point_step + mesh.cloud.fields[f].offset], &value, sizeof (float));
|
|
}
|
|
|
|
++v_idx;
|
|
}
|
|
catch (const boost::bad_lexical_cast &e)
|
|
{
|
|
log_<<"Unable to convert %s to vertex coordinates!\n";
|
|
throw OdmOrthoPhotoException("Unable to convert %s to vertex coordinates!");
|
|
}
|
|
continue;
|
|
}
|
|
// Vertex normal
|
|
if (st[0] == "vn")
|
|
{
|
|
try
|
|
{
|
|
for (int i = 1, f = normal_x_field; i < 4; ++i, ++f)
|
|
{
|
|
float value = boost::lexical_cast<float> (st[i]);
|
|
memcpy (&mesh.cloud.data[vn_idx * mesh.cloud.point_step + mesh.cloud.fields[f].offset],
|
|
&value,
|
|
sizeof (float));
|
|
}
|
|
++vn_idx;
|
|
}
|
|
catch (const boost::bad_lexical_cast &e)
|
|
{
|
|
log_<<"Unable to convert %s to vertex normal!\n";
|
|
throw OdmOrthoPhotoException("Unable to convert %s to vertex normal!");
|
|
}
|
|
continue;
|
|
}
|
|
// Texture coordinates
|
|
if (st[0] == "vt")
|
|
{
|
|
try
|
|
{
|
|
Eigen::Vector3f c (0, 0, 0);
|
|
for (std::size_t i = 1; i < st.size (); ++i)
|
|
c[i-1] = boost::lexical_cast<float> (st[i]);
|
|
|
|
if (c[2] == 0)
|
|
coordinates.push_back (Eigen::Vector2f (c[0], c[1]));
|
|
else
|
|
coordinates.push_back (Eigen::Vector2f (c[0]/c[2], c[1]/c[2]));
|
|
++vt_idx;
|
|
|
|
}
|
|
catch (const boost::bad_lexical_cast &e)
|
|
{
|
|
log_<<"Unable to convert %s to vertex texture coordinates!\n";
|
|
throw OdmOrthoPhotoException("Unable to convert %s to vertex texture coordinates!");
|
|
}
|
|
continue;
|
|
}
|
|
// Material
|
|
if (st[0] == "usemtl")
|
|
{
|
|
mesh.tex_polygons.push_back (std::vector<pcl::Vertices> ());
|
|
mesh.tex_materials.push_back (pcl::TexMaterial ());
|
|
for (std::size_t i = 0; i < companions_.size (); ++i)
|
|
{
|
|
std::vector<pcl::TexMaterial>::const_iterator mat_it = companions_[i].getMaterial (st[1]);
|
|
if (mat_it != companions_[i].materials_.end ())
|
|
{
|
|
mesh.tex_materials.back () = *mat_it;
|
|
break;
|
|
}
|
|
}
|
|
// We didn't find the appropriate material so we create it here with name only.
|
|
if (mesh.tex_materials.back ().tex_name == "")
|
|
mesh.tex_materials.back ().tex_name = st[1];
|
|
mesh.tex_coordinates.push_back (coordinates);
|
|
coordinates.clear ();
|
|
continue;
|
|
}
|
|
// Face
|
|
if (st[0] == "f")
|
|
{
|
|
//We only care for vertices indices
|
|
pcl::Vertices face_v; face_v.vertices.resize (st.size () - 1);
|
|
for (std::size_t i = 1; i < st.size (); ++i)
|
|
{
|
|
int v;
|
|
sscanf (st[i].c_str (), "%d", &v);
|
|
v = (v < 0) ? v_idx + v : v - 1;
|
|
face_v.vertices[i-1] = v;
|
|
|
|
int v2, vt, vn;
|
|
sscanf (st[i].c_str (), "%d/%d/%d", &v2, &vt, &vn);
|
|
f2vt[3*(f_idx) + i-1] = vt-1;
|
|
}
|
|
mesh.tex_polygons.back ().push_back (face_v);
|
|
++f_idx;
|
|
continue;
|
|
}
|
|
}
|
|
}
|
|
catch (const char *exception)
|
|
{
|
|
fs.close ();
|
|
log_<<"Unable to read file!\n";
|
|
throw OdmOrthoPhotoException("Unable to read file!");
|
|
}
|
|
|
|
if (vt_idx != v_idx)
|
|
{
|
|
std::vector<Eigen::Vector2f, Eigen::aligned_allocator<Eigen::Vector2f> > texcoordinates = std::vector<Eigen::Vector2f, Eigen::aligned_allocator<Eigen::Vector2f> >(0);
|
|
texcoordinates.reserve(3*f_idx);
|
|
|
|
for (size_t faceIndex = 0; faceIndex < f_idx; ++faceIndex)
|
|
{
|
|
for(size_t i = 0; i < 3; ++i)
|
|
{
|
|
Eigen::Vector2f vt = mesh.tex_coordinates[0][f2vt[3*faceIndex+i]];
|
|
texcoordinates.push_back(vt);
|
|
}
|
|
}
|
|
|
|
mesh.tex_coordinates.clear();
|
|
mesh.tex_coordinates.push_back(texcoordinates);
|
|
}
|
|
|
|
fs.close();
|
|
return (0);
|
|
}
|
|
|
|
bool OdmOrthoPhoto::readHeader (const std::string &file_name, pcl::PCLPointCloud2 &cloud,
|
|
Eigen::Vector4f &origin, Eigen::Quaternionf &orientation,
|
|
int &file_version, int &data_type, unsigned int &data_idx,
|
|
const int offset)
|
|
{
|
|
origin = Eigen::Vector4f::Zero ();
|
|
orientation = Eigen::Quaternionf::Identity ();
|
|
file_version = 0;
|
|
cloud.width = cloud.height = cloud.point_step = cloud.row_step = 0;
|
|
cloud.data.clear ();
|
|
data_type = 0;
|
|
data_idx = offset;
|
|
|
|
std::ifstream fs;
|
|
std::string line;
|
|
|
|
if (file_name == "" || !boost::filesystem::exists (file_name))
|
|
{
|
|
return false;
|
|
}
|
|
|
|
// Open file in binary mode to avoid problem of
|
|
// std::getline() corrupting the result of ifstream::tellg()
|
|
fs.open (file_name.c_str (), std::ios::binary);
|
|
if (!fs.is_open () || fs.fail ())
|
|
{
|
|
fs.close ();
|
|
return false;
|
|
}
|
|
|
|
// Seek at the given offset
|
|
fs.seekg (offset, std::ios::beg);
|
|
|
|
// Read the header and fill it in with wonderful values
|
|
bool vertex_normal_found = false;
|
|
bool vertex_texture_found = false;
|
|
// Material library, skip for now!
|
|
// bool material_found = false;
|
|
std::vector<std::string> material_files;
|
|
std::size_t nr_point = 0;
|
|
std::vector<std::string> st;
|
|
|
|
try
|
|
{
|
|
while (!fs.eof ())
|
|
{
|
|
getline (fs, line);
|
|
// Ignore empty lines
|
|
if (line == "")
|
|
continue;
|
|
|
|
// Tokenize the line
|
|
std::stringstream sstream (line);
|
|
sstream.imbue (std::locale::classic ());
|
|
line = sstream.str ();
|
|
boost::trim (line);
|
|
boost::split (st, line, boost::is_any_of ("\t\r "), boost::token_compress_on);
|
|
// Ignore comments
|
|
if (st.at (0) == "#")
|
|
continue;
|
|
|
|
// Vertex
|
|
if (st.at (0) == "v")
|
|
{
|
|
++nr_point;
|
|
continue;
|
|
}
|
|
|
|
// Vertex texture
|
|
if ((st.at (0) == "vt") && !vertex_texture_found)
|
|
{
|
|
vertex_texture_found = true;
|
|
continue;
|
|
}
|
|
|
|
// Vertex normal
|
|
if ((st.at (0) == "vn") && !vertex_normal_found)
|
|
{
|
|
vertex_normal_found = true;
|
|
continue;
|
|
}
|
|
|
|
// Material library, skip for now!
|
|
if (st.at (0) == "mtllib")
|
|
{
|
|
material_files.push_back (st.at (1));
|
|
continue;
|
|
}
|
|
}
|
|
}
|
|
catch (const char *exception)
|
|
{
|
|
fs.close ();
|
|
return false;
|
|
}
|
|
|
|
if (!nr_point)
|
|
{
|
|
fs.close ();
|
|
return false;
|
|
}
|
|
|
|
int field_offset = 0;
|
|
for (int i = 0; i < 3; ++i, field_offset += 4)
|
|
{
|
|
cloud.fields.push_back (pcl::PCLPointField ());
|
|
cloud.fields[i].offset = field_offset;
|
|
cloud.fields[i].datatype = pcl::PCLPointField::FLOAT32;
|
|
cloud.fields[i].count = 1;
|
|
}
|
|
|
|
cloud.fields[0].name = "x";
|
|
cloud.fields[1].name = "y";
|
|
cloud.fields[2].name = "z";
|
|
|
|
if (vertex_normal_found)
|
|
{
|
|
std::string normals_names[3] = { "normal_x", "normal_y", "normal_z" };
|
|
for (int i = 0; i < 3; ++i, field_offset += 4)
|
|
{
|
|
cloud.fields.push_back (pcl::PCLPointField ());
|
|
pcl::PCLPointField& last = cloud.fields.back ();
|
|
last.name = normals_names[i];
|
|
last.offset = field_offset;
|
|
last.datatype = pcl::PCLPointField::FLOAT32;
|
|
last.count = 1;
|
|
}
|
|
}
|
|
|
|
if (material_files.size () > 0)
|
|
{
|
|
for (std::size_t i = 0; i < material_files.size (); ++i)
|
|
{
|
|
pcl::MTLReader companion;
|
|
|
|
if (companion.read (file_name, material_files[i]))
|
|
{
|
|
log_<<"Problem reading material file.";
|
|
}
|
|
|
|
companions_.push_back (companion);
|
|
}
|
|
}
|
|
|
|
cloud.point_step = field_offset;
|
|
cloud.width = nr_point;
|
|
cloud.height = 1;
|
|
cloud.row_step = cloud.point_step * cloud.width;
|
|
cloud.is_dense = true;
|
|
cloud.data.resize (cloud.point_step * nr_point);
|
|
fs.close ();
|
|
return true;
|
|
}
|