void Asteroid::moveStep(float delta)
{
FlyingObject::moveStep(delta);
rotateAngle = rotateAngle + _rotateSpeed * delta;
rotatePoints(vertices, rotatedVertices, rotateAngle , nvertices);
if (_bonus)
_bonus->moveStep(delta);
}
bool Scanner::processScan(std::vector<NeutralFileRecord> & results, int frame, float rotation, LocationMapper& locMapper, Laser::LaserSide laserSide, int & firstRowLaserCol, TimingStats * timingStats)
{
int numLocationsMapped = 0;
real percentPixelsOverThreshold = 0;
// Send the pictures off for processing
double time1 = GetTimeInSeconds();
int numLocations = m_imageProcessor.process(m_image1,
m_image2,
NULL,
m_laserLocations,
m_maxNumLocations,
firstRowLaserCol,
percentPixelsOverThreshold,
NULL);
timingStats->imageProcessingTime += GetTimeInSeconds() - time1;
// If we had major problems with this frame, try it again
std::cout << "percentPixelsOverThreshold: " << percentPixelsOverThreshold << std::endl;
if (percentPixelsOverThreshold > m_maxPercentPixelsOverThreshold)
{
std::cout << "!! Many bad laser locations suspected, pctOverThreshold=" << percentPixelsOverThreshold
<< ", maxPct=" << m_maxPercentPixelsOverThreshold << ", rescanning..." << std::endl;
timingStats->numFrameRetries++;
return false;
}
std::cout << "Detected " << numLocations << " laser pixels." << std::endl;
// Lookup the 3D locations for the laser points
numLocationsMapped = 0;
if (numLocations > 0)
{
time1 = GetTimeInSeconds();
locMapper.mapPoints(m_laserLocations, &m_image1, m_columnPoints, numLocations, numLocationsMapped);
timingStats->pointMappingTime += GetTimeInSeconds() - time1;
if (numLocations != numLocationsMapped)
{
std::cout << "Discarded " << numLocations - numLocationsMapped << " points." << std::endl;
}
}
else
{
// Stop here if we didn't detect the laser at all
std::cerr << "!!! Could not detect laser at all" << std::endl;
}
// Map the points if there was something to map
if (numLocationsMapped > 0)
{
time1 = GetTimeInSeconds();
if (m_writeRangeCsvEnabled)
{
writeRangePoints(m_columnPoints, numLocationsMapped, laserSide);
}
// Rotate the points
rotatePoints(m_columnPoints, rotation, numLocationsMapped);
// Write to the neutral file
time1 = GetTimeInSeconds();
m_results.enter();
for (int iLoc = 0; iLoc < numLocationsMapped; iLoc++)
{
NeutralFileRecord record;
record.pixel = m_laserLocations[iLoc];
record.point = m_columnPoints[iLoc];
record.rotation = laserSide == Laser::RIGHT_LASER ? rotation : rotation + m_radiansBetweenLaserPlanes;
record.frame = frame;
record.laserSide = (int) laserSide;
results.push_back(record);
}
m_results.leave();
timingStats->pointProcessingTime += GetTimeInSeconds() - time1;
}
return true;
}
//********************************
//* main
int main( int argc, char* argv[])
{
if( argc != 4 ) {
std::cerr << "usage: " << argv[0] << " [path] [label] <registration_num>" << std::endl;
exit( EXIT_FAILURE );
}
const int file_num = atoi( argv[3] );
char tmpname[ 1000 ];
//* length of bounding box sides (initialize)
float size1 = 0;
float size2 = 0;
float size3 = 0;
float vals[3];
//* read rotate_num
sprintf( tmpname, "%s/param/parameters.txt", argv[1] );
const int rotate_num = Param::readRotateNum( tmpname );
//* read the number of voxels in each subdivision's side of a target object
const int subdivision_size = Param::readBoxSizeModel( tmpname );
//* read the length of voxel side
const float voxel_size = Param::readVoxelSize( tmpname );
//* read the threshold for RGB binalize
int color_threshold_r, color_threshold_g, color_threshold_b;
sprintf( tmpname, "%s/param/color_threshold.txt", argv[1] );
Param::readColorThreshold( color_threshold_r, color_threshold_g, color_threshold_b, tmpname );
//* Voxel Grid
pcl::VoxelGrid<pcl::PointXYZRGB> grid;
grid.setLeafSize (voxel_size, voxel_size, voxel_size);
grid.setSaveLeafLayout(true);
//******************************//
//* C3-HLAC feature extraction *//
//******************************//
int write_count = 0;
std::vector< std::vector<float> > c3_hlac;
for( int n = 0; n < file_num; n++ ) {
printf("%d in %d...\n",n,file_num);
pcl::PointCloud<pcl::PointXYZRGB> ref_cloud;
sprintf( tmpname, "%s/models/%s/Points/%05d.pcd", argv[1], argv[2], n );
pcl::io::loadPCDFile (tmpname, ref_cloud);
//* rotate point clouds synthetically to T postures
//* T postures: obtained by rotation with each (90/$rotate_num) degrees for x-, y- and z-axis.
for(int r3=0; r3 < rotate_num; r3++) {
for(int r2=0; r2 < rotate_num; r2++) {
for(int r1=0; r1 < rotate_num; r1++) {
const double roll = r3 * M_PI / (2*rotate_num);
const double pan = r2 * M_PI / (2*rotate_num);
const double roll2 = r1 * M_PI / (2*rotate_num);
//* voxelize
pcl::PointCloud<pcl::PointXYZRGB> input_cloud;
pcl::PointCloud<pcl::PointXYZRGB> cloud_downsampled;
rotatePoints( ref_cloud, input_cloud, roll, pan, roll2 ); // rotation
grid.setInputCloud ( input_cloud.makeShared() );
grid.filter (cloud_downsampled);
//* extract features
extractC3HLACSignature981( grid, cloud_downsampled, c3_hlac, color_threshold_r, color_threshold_g, color_threshold_b, voxel_size, subdivision_size );
sprintf( tmpname, "%s/models/%s/Features/%05d.pcd", argv[1], argv[2], write_count++ );
writeFeature( tmpname, c3_hlac );
//* determine bounding box size
getMinMax_points( cloud_downsampled, vals[0], vals[1], vals[2] );
float tmp_size1 = 0;
float tmp_size2 = 0;
float tmp_size3 = 0;
for( int j=0; j<3; j++ )
setSize( tmp_size1, tmp_size2, tmp_size3, vals[ j ] );
if( tmp_size1 > size1 ) size1 = tmp_size1;
if( tmp_size2 > size2 ) size2 = tmp_size2;
if( tmp_size3 > size3 ) size3 = tmp_size3;
if(r2==0) break;
}
}
}
}
//* output bounding box size
sprintf( tmpname, "%s/models/%s/size.txt",argv[1], argv[2] );
FILE *fp = fopen( tmpname, "w" );
fprintf( fp, "%f %f %f\n", size1, size2, size3 );
fclose( fp );
return 0;
}
//********************************
//* main
int main(int argc, char **argv)
{
if( argc != 4 ){
std::cerr << "usage: " << argv[0] << " [path] [model_name] <registration_num>" << std::endl;
exit( EXIT_FAILURE );
}
char tmpname[ 1000 ];
//* length of bounding box sides (initialize)
float size1 = 0;
float size2 = 0;
float size3 = 0;
float vals[3];
//* read rotate_num
sprintf( tmpname, "%s/param/parameters.txt", argv[1] );
const int rotate_num = Param::readRotateNum( tmpname );
//* read the number of voxels in each subdivision's side of a target object
const float voxel_size = Param::readVoxelSize( tmpname );
//* Voxel Grid
pcl::VoxelGrid<pcl::PointXYZRGB> grid;
grid.setLeafSize (voxel_size, voxel_size, voxel_size);
grid.setSaveLeafLayout(true);
const int obj_num = atoi(argv[3]);
int write_count = 0; // number for output file names
for( int i=0; i<obj_num; i++ ){
pcl::PointCloud<pcl::PointXYZRGB> ref_cloud;
sprintf( tmpname, "%s/models/%s/Points/%05d.pcd", argv[1], argv[2], i );
pcl::io::loadPCDFile (tmpname, ref_cloud);
//* rotate point clouds synthetically to T postures
//* T postures: obtained by rotation with each (90/$rotate_num) degrees for x-, y- and z-axis.
for(int r3=0; r3 < rotate_num; r3++){
for(int r2=0; r2 < rotate_num; r2++){
for(int r1=0; r1 < rotate_num; r1++){
const double roll = r3 * M_PI / (2*rotate_num);
const double pan = r2 * M_PI / (2*rotate_num);
const double roll2 = r1 * M_PI / (2*rotate_num);
//* voxelize
pcl::PointCloud<pcl::PointXYZRGB> input_cloud;
pcl::PointCloud<pcl::PointXYZRGB> output_cloud;
rotatePoints( ref_cloud, input_cloud, roll, pan, roll2 ); // rotation
grid.setInputCloud ( input_cloud.makeShared() );
grid.filter (output_cloud);
sprintf( tmpname, "%s/models/%s/Voxel/%05d.dat", argv[1], argv[2], write_count++ );
pcl::io::savePCDFile (tmpname, output_cloud, true);
//* determine bounding box size
getMinMax_points( output_cloud, vals[0], vals[1], vals[2] );
float tmp_size1 = 0;
float tmp_size2 = 0;
float tmp_size3 = 0;
for( int j=0; j<3; j++ )
setSize( tmp_size1, tmp_size2, tmp_size3, vals[ j ] );
if( tmp_size1 > size1 ) size1 = tmp_size1;
if( tmp_size2 > size2 ) size2 = tmp_size2;
if( tmp_size3 > size3 ) size3 = tmp_size3;
if(r2==0) break;
}
}
}
}
//* output bounding box size
sprintf( tmpname, "%s/models/%s/size.txt", argv[1], argv[2] );
FILE *fp = fopen( tmpname, "w" );
fprintf( fp, "%f %f %f\n", size1, size2, size3 );
fclose( fp );
return(0);
}