int MatrixMatrixMultiply( double *A, int hA, int wA, double *B, int hB, int wB, double *C)
{
/* Check that Matrix deminsions are valid */
if( wA != hB){
{
fprintf(stderr, "Matrix Multiplication Invalid! \nThe number of columns of A must equal the number of rows of B!\n");
abort();
}
}
/* Matrix Deminsions of Output matrix */
int wC = wB;
int hC = hA;
CleanMatrix(C, hC, wC);
/* Calculate the number of Blocks in the height and width of C, as well as the width of A */
int wn_bloc = (wC+L2_BLK_SIZE -1)/L2_BLK_SIZE; // Number of Blocks in the widith (round up)
int hn_bloc = (hC+L2_BLK_SIZE -1)/L2_BLK_SIZE; // Number of Blocks in the height (round up)
int wA_bloc = (wA+L2_BLK_SIZE -1)/L2_BLK_SIZE; // Number of Blocks in the widith of matrix A (round up)
int b = L2_BLK_SIZE; // Block Size
/* Make static matrices */
static __attribute__ ((aligned(16))) double a_block[L2_BLK_SIZE*L2_BLK_SIZE];
static __attribute__ ((aligned(16))) double b_block[L2_BLK_SIZE*L2_BLK_SIZE];
static __attribute__ ((aligned(16))) double c_block[L2_BLK_SIZE*L2_BLK_SIZE];
/* Clean matrices */
CleanMatrix(c_block, b, b);
CleanMatrix(a_block, b, b);
CleanMatrix(b_block, b, b);
for( int i = 0; i < hn_bloc; i++){
for( int j=0; j < wn_bloc; j++){
BlockMatrix(C, c_block, hC, wC, b, i, j);
for( int k = 0; k < wA_bloc ; k++){
BlockMatrix(A, a_block, hA, wA, b, i, k);
BlockMatrix(B, b_block, hB, wB, b, k, j);
dgemm_middle(a_block, b_block, c_block);
CleanMatrix(a_block, b, b);
CleanMatrix(b_block, b, b);
}
UnBlockMatrix(C, c_block, hC, wC, b, i,j);
CleanMatrix(c_block, b, b);
}
}
/* Uncomment to Test this function */
//testMatrixMultiply(A, B, C, hA, wA, hB, wB);
return 0;
}
int MatrixVectorMultiply(const double * const A, const int hA, const int wA,
const double * const B, double *C){
int hB = wA;
/* Matrix Deminsions of Output matrix */
int wC = 1;
int hC = hA;
CleanMatrix(C, hC, wC);
/* Calculate the number of Blocks in the width of A */
int wn_bloc = (wA + L2_BLK_SIZE - 1) / L2_BLK_SIZE;
// Number of Blocks in the height (round up)
int hn_bloc = (hC+L2_BLK_SIZE -1)/L2_BLK_SIZE;
// Number of Blocks in the widith of matrix A (round up)
int b = L2_BLK_SIZE; // Block Size
/* Make static matrices */
static __attribute__ ((aligned(16))) double a_block[L2_BLK_SIZE*L2_BLK_SIZE];
static __attribute__ ((aligned(16))) double b_block[L2_BLK_SIZE];
static __attribute__ ((aligned(16))) double c_block[L2_BLK_SIZE];
/* Clean matrices */
CleanMatrix(c_block, b, 1);
CleanMatrix(a_block, b, b);
CleanMatrix(b_block, b, 1);
for(int i = 0; i < hn_bloc; i++){
for(int j = 0; j < wn_bloc; j++){
BlockVectorForMatrixVector(C, c_block, hC, b, i);
BlockMatrix(A, a_block, hA, wA, b, i, j);
BlockVectorForMatrixVector(B, b_block, hB, b, j);
if(verbose) printf("\n=top=i:%d j:%d a_block: \n",i,j);
prettyPrint(a_block, b, b);
if(verbose) printf("\n=top=i:%d j:%d b_block: \n", i,j);
prettyPrint(b_block, b, 1);
dgemm_vector_middle(a_block, b_block, c_block, i, j, wA, hA);
CleanMatrix(a_block, b, b);
CleanMatrix(b_block, b, 1);
UnBlockVectorForMatrixVector(C, c_block, hC, b, i);
if(verbose) printf("\n=top=C:\n");
prettyPrint(C, wA, 1);
if(verbose) printf("\n=top= c_block:\n");
prettyPrint(c_block, b, 1);
CleanMatrix(c_block, b, b);
}
}
/* Uncomment to Test this function */
//testMatrixMultiply(A, B, C, hA, wA, hB, 1);
return 0;
}
CC_FILE_ERROR PTXFilter::loadFile( QString filename,
ccHObject& container,
LoadParameters& parameters)
{
//open ASCII file for reading
QFile file(filename);
if (!file.open(QIODevice::ReadOnly | QIODevice::Text))
{
return CC_FERR_READING;
}
QTextStream inFile(&file);
CCVector3d PshiftTrans(0,0,0);
CCVector3d PshiftCloud(0,0,0);
CC_FILE_ERROR result = CC_FERR_NO_LOAD;
ScalarType minIntensity = 0;
ScalarType maxIntensity = 0;
//progress dialog
ccProgressDialog pdlg(true, parameters.parentWidget);
pdlg.setMethodTitle(QObject::tr("Loading PTX file"));
pdlg.setAutoClose(false);
//progress dialog (for normals computation)
ccProgressDialog normalsProgressDlg(true, parameters.parentWidget);
normalsProgressDlg.setAutoClose(false);
for (unsigned cloudIndex = 0; result == CC_FERR_NO_ERROR || result == CC_FERR_NO_LOAD; cloudIndex++)
{
unsigned width = 0, height = 0;
ccGLMatrixd sensorTransD, cloudTransD;
//read header
{
QString line = inFile.readLine();
if (line.isNull() && container.getChildrenNumber() != 0) //end of file?
break;
//read the width (number of columns) and the height (number of rows) on the two first lines
//(DGM: we transpose the matrix right away)
bool ok;
height = line.toUInt(&ok);
if (!ok)
return CC_FERR_MALFORMED_FILE;
line = inFile.readLine();
width = line.toUInt(&ok);
if (!ok)
return CC_FERR_MALFORMED_FILE;
ccLog::Print(QString("[PTX] Scan #%1 - grid size: %2 x %3").arg(cloudIndex+1).arg(height).arg(width));
//read sensor transformation matrix
for (int i=0; i<4; ++i)
{
line = inFile.readLine();
QStringList tokens = line.split(" ",QString::SkipEmptyParts);
if (tokens.size() != 3)
return CC_FERR_MALFORMED_FILE;
double* colDest = 0;
if (i == 0)
{
//Translation
colDest = sensorTransD.getTranslation();
}
else
{
//X, Y and Z axis
colDest = sensorTransD.getColumn(i-1);
}
for (int j=0; j<3; ++j)
{
assert(colDest);
colDest[j] = tokens[j].toDouble(&ok);
if (!ok)
return CC_FERR_MALFORMED_FILE;
}
}
//make the transform a little bit cleaner (necessary as it's read from ASCII!)
CleanMatrix(sensorTransD);
//read cloud transformation matrix
for (int i=0; i<4; ++i)
{
line = inFile.readLine();
QStringList tokens = line.split(" ",QString::SkipEmptyParts);
if (tokens.size() != 4)
return CC_FERR_MALFORMED_FILE;
double* col = cloudTransD.getColumn(i);
for (int j=0; j<4; ++j)
{
col[j] = tokens[j].toDouble(&ok);
if (!ok)
return CC_FERR_MALFORMED_FILE;
}
}
//make the transform a little bit cleaner (necessary as it's read from ASCII!)
CleanMatrix(cloudTransD);
//handle Global Shift directly on the first cloud's translation!
if (cloudIndex == 0)
{
if (HandleGlobalShift(cloudTransD.getTranslationAsVec3D(),PshiftTrans,parameters))
{
ccLog::Warning("[PTXFilter::loadFile] Cloud has be recentered! Translation: (%.2f,%.2f,%.2f)",PshiftTrans.x,PshiftTrans.y,PshiftTrans.z);
}
}
//'remove' global shift from the sensor and cloud transformation matrices
cloudTransD.setTranslation(cloudTransD.getTranslationAsVec3D() + PshiftTrans);
sensorTransD.setTranslation(sensorTransD.getTranslationAsVec3D() + PshiftTrans);
}
//now we can read the grid cells
ccPointCloud* cloud = new ccPointCloud();
if (container.getChildrenNumber() == 0)
{
cloud->setName("unnamed - Cloud");
}
else
{
if (container.getChildrenNumber() == 1)
container.getChild(0)->setName("unnamed - Cloud 1"); //update previous cloud name!
cloud->setName(QString("unnamed - Cloud %1").arg(container.getChildrenNumber()+1));
}
unsigned gridSize = width * height;
if (!cloud->reserve(gridSize))
{
result = CC_FERR_NOT_ENOUGH_MEMORY;
delete cloud;
cloud = 0;
break;
}
//set global shift
cloud->setGlobalShift(PshiftTrans);
//intensities
ccScalarField* intensitySF = new ccScalarField(CC_PTX_INTENSITY_FIELD_NAME);
if (!intensitySF->reserve(static_cast<unsigned>(gridSize)))
{
ccLog::Warning("[PTX] Not enough memory to load intensities!");
intensitySF->release();
intensitySF = 0;
}
//grid structure
ccPointCloud::Grid::Shared grid(new ccPointCloud::Grid);
grid->w = width;
grid->h = height;
bool hasIndexGrid = true;
try
{
grid->indexes.resize(gridSize,-1); //-1 means no cell/point
}
catch (const std::bad_alloc&)
{
ccLog::Warning("[PTX] Not enough memory to load the grid structure");
hasIndexGrid = false;
}
//read points
{
CCLib::NormalizedProgress nprogress(&pdlg, gridSize);
pdlg.setInfo(qPrintable(QString("Number of cells: %1").arg(gridSize)));
pdlg.start();
bool firstPoint = true;
bool hasColors = false;
bool loadColors = false;
bool loadGridColors = false;
size_t gridIndex = 0;
for (unsigned j=0; j<height; ++j)
{
for (unsigned i=0; i<width; ++i, ++gridIndex)
{
QString line = inFile.readLine();
QStringList tokens = line.split(" ",QString::SkipEmptyParts);
if (firstPoint)
{
hasColors = (tokens.size() == 7);
if (hasColors)
{
loadColors = cloud->reserveTheRGBTable();
if (!loadColors)
{
ccLog::Warning("[PTX] Not enough memory to load RGB colors!");
}
else if (hasIndexGrid)
{
//we also load the colors into the grid (as invalid/missing points can have colors!)
try
{
grid->colors.resize(gridSize, ccColor::Rgb(0, 0, 0));
loadGridColors = true;
}
catch (const std::bad_alloc&)
{
ccLog::Warning("[PTX] Not enough memory to load the grid colors");
}
}
}
}
if ((hasColors && tokens.size() != 7) || (!hasColors && tokens.size() != 4))
{
result = CC_FERR_MALFORMED_FILE;
//early stop
j = height;
break;
}
double values[4];
for (int v=0; v<4; ++v)
{
bool ok;
values[v] = tokens[v].toDouble(&ok);
if (!ok)
{
result = CC_FERR_MALFORMED_FILE;
//early stop
j = height;
break;
}
}
//we skip "empty" cells
bool pointIsValid = (CCVector3d::fromArray(values).norm2() != 0);
if (pointIsValid)
{
const double* Pd = values;
//first point: check for 'big' coordinates
if (firstPoint)
{
if (cloudIndex == 0 && !cloud->isShifted()) //in case the trans. matrix was ok!
{
CCVector3d P(Pd);
if (HandleGlobalShift(P,PshiftCloud,parameters))
{
cloud->setGlobalShift(PshiftCloud);
ccLog::Warning("[PTXFilter::loadFile] Cloud has been recentered! Translation: (%.2f,%.2f,%.2f)",PshiftCloud.x,PshiftCloud.y,PshiftCloud.z);
}
}
firstPoint = false;
}
//update index grid
if (hasIndexGrid)
{
grid->indexes[gridIndex] = static_cast<int>(cloud->size()); // = index (default value = -1, means no point)
}
//add point
cloud->addPoint(CCVector3( static_cast<PointCoordinateType>(Pd[0] + PshiftCloud.x),
static_cast<PointCoordinateType>(Pd[1] + PshiftCloud.y),
static_cast<PointCoordinateType>(Pd[2] + PshiftCloud.z)) );
//add intensity
if (intensitySF)
{
intensitySF->addElement(static_cast<ScalarType>(values[3]));
}
}
//color
if (loadColors && (pointIsValid || loadGridColors))
{
ccColor::Rgb color;
for (int c=0; c<3; ++c)
{
bool ok;
unsigned temp = tokens[4+c].toUInt(&ok);
ok &= (temp <= 255);
if (ok)
{
color.rgb[c] = static_cast<unsigned char>(temp);
}
else
{
result = CC_FERR_MALFORMED_FILE;
//early stop
j = height;
break;
}
}
if (pointIsValid)
{
cloud->addRGBColor(color.rgb);
}
if (loadGridColors)
{
assert(!grid->colors.empty());
grid->colors[gridIndex] = color;
}
}
if (!nprogress.oneStep())
{
result = CC_FERR_CANCELED_BY_USER;
break;
}
}
}
}
//is there at least one valid point in this grid?
if (cloud->size() == 0)
{
delete cloud;
cloud = 0;
if (intensitySF)
intensitySF->release();
ccLog::Warning(QString("[PTX] Scan #%1 is empty?!").arg(cloudIndex+1));
}
else
{
if (result == CC_FERR_NO_LOAD)
result = CC_FERR_NO_ERROR; //to make clear that we have loaded at least something!
cloud->resize(cloud->size());
if (intensitySF)
{
assert(intensitySF->currentSize() == cloud->size());
intensitySF->resize(cloud->size());
intensitySF->computeMinAndMax();
int intensitySFIndex = cloud->addScalarField(intensitySF);
//keep track of the min and max intensity
if (container.getChildrenNumber() == 0)
{
minIntensity = intensitySF->getMin();
maxIntensity = intensitySF->getMax();
}
else
{
minIntensity = std::min(minIntensity,intensitySF->getMin());
maxIntensity = std::max(maxIntensity,intensitySF->getMax());
}
cloud->showSF(true);
cloud->setCurrentDisplayedScalarField(intensitySFIndex);
}
ccGBLSensor* sensor = 0;
if (hasIndexGrid && result != CC_FERR_CANCELED_BY_USER)
{
//determine best sensor parameters (mainly yaw and pitch steps)
ccGLMatrix cloudToSensorTrans((sensorTransD.inverse() * cloudTransD).data());
sensor = ccGriddedTools::ComputeBestSensor(cloud, grid, &cloudToSensorTrans);
}
//we apply the transformation
ccGLMatrix cloudTrans(cloudTransD.data());
cloud->applyGLTransformation_recursive(&cloudTrans);
if (sensor)
{
ccGLMatrix sensorTrans(sensorTransD.data());
sensor->setRigidTransformation(sensorTrans); //after cloud->applyGLTransformation_recursive!
cloud->addChild(sensor);
}
//scan grid
if (hasIndexGrid)
{
grid->validCount = static_cast<unsigned>(cloud->size());
grid->minValidIndex = 0;
grid->maxValidIndex = grid->validCount-1;
grid->sensorPosition = sensorTransD;
cloud->addGrid(grid);
//by default we don't compute normals without asking the user
if (parameters.autoComputeNormals)
{
cloud->computeNormalsWithGrids(LS, 2, true, &normalsProgressDlg);
}
}
cloud->setVisible(true);
cloud->showColors(cloud->hasColors());
cloud->showNormals(cloud->hasNormals());
container.addChild(cloud);
#ifdef QT_DEBUG
//break;
#endif
}
}
//update scalar fields saturation (globally!)
{
bool validIntensityRange = true;
if (minIntensity < 0 || maxIntensity > 1.0)
{
ccLog::Warning("[PTX] Intensity values are invalid (they should all fall in [0 ; 1])");
validIntensityRange = false;
}
for (unsigned i=0; i<container.getChildrenNumber(); ++i)
{
ccHObject* obj = container.getChild(i);
assert(obj && obj->isA(CC_TYPES::POINT_CLOUD));
CCLib::ScalarField* sf = static_cast<ccPointCloud*>(obj)->getScalarField(0);
if (sf)
{
ccScalarField* ccSF = static_cast<ccScalarField*>(sf);
ccSF->setColorScale(ccColorScalesManager::GetDefaultScale(validIntensityRange ? ccColorScalesManager::ABS_NORM_GREY : ccColorScalesManager::GREY));
ccSF->setSaturationStart(0/*minIntensity*/);
ccSF->setSaturationStop(maxIntensity);
}
}
}
return result;
}
