void time_dispatch_hsa_kernel(int dispatch_count, const grid_launch_parm *lp, const char *nullkernel_hsaco)
{
Kernel k = load_hsaco(lp->av, nullkernel_hsaco, KERNEL_NAME);
std::chrono::time_point<std::chrono::high_resolution_clock> start, end;
const char *testName = "dispatch_hsa_kernel";
std::vector<std::chrono::duration<double>> elapsed_timer;
// Timing null grid_launch call, active wait
for(int i = 0; i < dispatch_count; ++i) {
start = std::chrono::high_resolution_clock::now();
explicit_launch_null_kernel(lp, k);
//std::cout << "CF get_use_count=" << cf.get_use_count() << "is_ready=" << cf.is_ready()<< "\n";
//
lp->av->wait(hc::hcWaitModeActive);
//cf.wait(hc::hcWaitModeActive);
end = std::chrono::high_resolution_clock::now();
std::chrono::duration<double> dur = end - start;
elapsed_timer.push_back(dur);
}
std::vector<std::chrono::duration<double>> outliers;
remove_outliers(elapsed_timer, outliers);
plot(testName, elapsed_timer);
std::cout << std::setw(20) << std::left << testName << "time, active (us): "
<< std::setprecision(8) << average(elapsed_timer)*1000000.0 << "\n";
};
void Panorama::run()
{
open_images();
calc_features();
create_matchs();
remove_outliers();
calc_camera_params();
calc_gains();
blend_images();
}
CPLXMLNode *GDALSerializeGCPTransformer( void *pTransformArg )
{
CPLXMLNode *psTree = NULL;
GCPTransformInfo *psInfo = (GCPTransformInfo *) pTransformArg;
VALIDATE_POINTER1( pTransformArg, "GDALSerializeGCPTransformer", NULL );
psTree = CPLCreateXMLNode( NULL, CXT_Element, "GCPTransformer" );
/* -------------------------------------------------------------------- */
/* Serialize Order and bReversed. */
/* -------------------------------------------------------------------- */
CPLCreateXMLElementAndValue(
psTree, "Order",
CPLSPrintf( "%d", psInfo->nOrder ) );
CPLCreateXMLElementAndValue(
psTree, "Reversed",
CPLSPrintf( "%d", psInfo->bReversed ) );
if( psInfo->bRefine )
{
CPLCreateXMLElementAndValue(
psTree, "Refine",
CPLSPrintf( "%d", psInfo->bRefine ) );
CPLCreateXMLElementAndValue(
psTree, "MinimumGcps",
CPLSPrintf( "%d", psInfo->nMinimumGcps ) );
CPLCreateXMLElementAndValue(
psTree, "Tolerance",
CPLSPrintf( "%f", psInfo->dfTolerance ) );
}
/* -------------------------------------------------------------------- */
/* Attach GCP List. */
/* -------------------------------------------------------------------- */
if( psInfo->nGCPCount > 0 )
{
if(psInfo->bRefine)
{
remove_outliers(psInfo);
}
GDALSerializeGCPListToXML( psTree,
psInfo->pasGCPList,
psInfo->nGCPCount,
NULL );
}
return psTree;
}
CPLXMLNode *GDALSerializeGCPTransformer( void *pTransformArg )
{
CPLXMLNode *psTree;
GCPTransformInfo *psInfo = (GCPTransformInfo *) pTransformArg;
VALIDATE_POINTER1( pTransformArg, "GDALSerializeGCPTransformer", NULL );
psTree = CPLCreateXMLNode( NULL, CXT_Element, "GCPTransformer" );
/* -------------------------------------------------------------------- */
/* Serialize Order and bReversed. */
/* -------------------------------------------------------------------- */
CPLCreateXMLElementAndValue(
psTree, "Order",
CPLSPrintf( "%d", psInfo->nOrder ) );
CPLCreateXMLElementAndValue(
psTree, "Reversed",
CPLSPrintf( "%d", psInfo->bReversed ) );
if( psInfo->bRefine )
{
CPLCreateXMLElementAndValue(
psTree, "Refine",
CPLSPrintf( "%d", psInfo->bRefine ) );
CPLCreateXMLElementAndValue(
psTree, "MinimumGcps",
CPLSPrintf( "%d", psInfo->nMinimumGcps ) );
CPLCreateXMLElementAndValue(
psTree, "Tolerance",
CPLSPrintf( "%f", psInfo->dfTolerance ) );
}
/* -------------------------------------------------------------------- */
/* Attach GCP List. */
/* -------------------------------------------------------------------- */
if( psInfo->nGCPCount > 0 )
{
int iGCP;
CPLXMLNode *psGCPList = CPLCreateXMLNode( psTree, CXT_Element,
"GCPList" );
if(psInfo->bRefine)
{
remove_outliers(psInfo);
}
for( iGCP = 0; iGCP < psInfo->nGCPCount; iGCP++ )
{
CPLXMLNode *psXMLGCP;
GDAL_GCP *psGCP = psInfo->pasGCPList + iGCP;
psXMLGCP = CPLCreateXMLNode( psGCPList, CXT_Element, "GCP" );
CPLSetXMLValue( psXMLGCP, "#Id", psGCP->pszId );
if( psGCP->pszInfo != NULL && strlen(psGCP->pszInfo) > 0 )
CPLSetXMLValue( psXMLGCP, "Info", psGCP->pszInfo );
CPLSetXMLValue( psXMLGCP, "#Pixel",
CPLSPrintf( "%.4f", psGCP->dfGCPPixel ) );
CPLSetXMLValue( psXMLGCP, "#Line",
CPLSPrintf( "%.4f", psGCP->dfGCPLine ) );
CPLSetXMLValue( psXMLGCP, "#X",
CPLSPrintf( "%.12E", psGCP->dfGCPX ) );
CPLSetXMLValue( psXMLGCP, "#Y",
CPLSPrintf( "%.12E", psGCP->dfGCPY ) );
if( psGCP->dfGCPZ != 0.0 )
CPLSetXMLValue( psXMLGCP, "#GCPZ",
CPLSPrintf( "%.12E", psGCP->dfGCPZ ) );
}
}
return psTree;
}
void *GDALCreateGCPTransformerEx( int nGCPCount, const GDAL_GCP *pasGCPList,
int nReqOrder, int bReversed, int bRefine, double dfTolerance, int nMinimumGcps)
{
GCPTransformInfo *psInfo;
double *padfGeoX, *padfGeoY, *padfRasterX, *padfRasterY;
int *panStatus, iGCP;
int nCRSresult;
struct Control_Points sPoints;
if( nReqOrder == 0 )
{
if( nGCPCount >= 10 )
nReqOrder = 2; /*for now we avoid 3rd order since it is unstable*/
else if( nGCPCount >= 6 )
nReqOrder = 2;
else
nReqOrder = 1;
}
psInfo = (GCPTransformInfo *) CPLCalloc(sizeof(GCPTransformInfo),1);
psInfo->bReversed = bReversed;
psInfo->nOrder = nReqOrder;
psInfo->bRefine = bRefine;
psInfo->dfTolerance = dfTolerance;
psInfo->nMinimumGcps = nMinimumGcps;
psInfo->pasGCPList = GDALDuplicateGCPs( nGCPCount, pasGCPList );
psInfo->nGCPCount = nGCPCount;
strcpy( psInfo->sTI.szSignature, "GTI" );
psInfo->sTI.pszClassName = "GDALGCPTransformer";
psInfo->sTI.pfnTransform = GDALGCPTransform;
psInfo->sTI.pfnCleanup = GDALDestroyGCPTransformer;
psInfo->sTI.pfnSerialize = GDALSerializeGCPTransformer;
/* -------------------------------------------------------------------- */
/* Compute the forward and reverse polynomials. */
/* -------------------------------------------------------------------- */
if(bRefine)
{
nCRSresult = remove_outliers(psInfo);
}
else
{
/* -------------------------------------------------------------------- */
/* Allocate and initialize the working points list. */
/* -------------------------------------------------------------------- */
padfGeoX = (double *) CPLCalloc(sizeof(double),nGCPCount);
padfGeoY = (double *) CPLCalloc(sizeof(double),nGCPCount);
padfRasterX = (double *) CPLCalloc(sizeof(double),nGCPCount);
padfRasterY = (double *) CPLCalloc(sizeof(double),nGCPCount);
panStatus = (int *) CPLCalloc(sizeof(int),nGCPCount);
for( iGCP = 0; iGCP < nGCPCount; iGCP++ )
{
panStatus[iGCP] = 1;
padfGeoX[iGCP] = pasGCPList[iGCP].dfGCPX;
padfGeoY[iGCP] = pasGCPList[iGCP].dfGCPY;
padfRasterX[iGCP] = pasGCPList[iGCP].dfGCPPixel;
padfRasterY[iGCP] = pasGCPList[iGCP].dfGCPLine;
}
sPoints.count = nGCPCount;
sPoints.e1 = padfRasterX;
sPoints.n1 = padfRasterY;
sPoints.e2 = padfGeoX;
sPoints.n2 = padfGeoY;
sPoints.status = panStatus;
nCRSresult = CRS_compute_georef_equations( &sPoints,
psInfo->adfToGeoX, psInfo->adfToGeoY,
psInfo->adfFromGeoX, psInfo->adfFromGeoY,
nReqOrder );
CPLFree( padfGeoX );
CPLFree( padfGeoY );
CPLFree( padfRasterX );
CPLFree( padfRasterY );
CPLFree( panStatus );
}
if (nCRSresult != 1)
{
CPLError( CE_Failure, CPLE_AppDefined, "%s", CRS_error_message[-nCRSresult]);
GDALDestroyGCPTransformer( psInfo );
return NULL;
}
else
{
return psInfo;
}
}
int fftMatch_gridded(char *inFile1, char *inFile2, char *gridFile,
float *avgLocX, float *avgLocY, float *certainty,
int size, double tol, int overlap)
{
meta_parameters *meta1 = meta_read(inFile1);
meta_parameters *meta2 = meta_read(inFile2);
int nl = mini(meta1->general->line_count, meta2->general->line_count);
int ns = mini(meta1->general->sample_count, meta2->general->sample_count);
if (size<0) size = 512;
if (overlap<0) overlap = 256;
if (tol<0) tol = .28;
asfPrintStatus("Tile size is %dx%d pixels\n", size, size);
asfPrintStatus("Tile overlap is %d pixels\n", overlap);
asfPrintStatus("Match tolerance is %.2f\n", tol);
long long lsz = (long long)size;
int num_x = (ns - size) / (size - overlap);
int num_y = (nl - size) / (size - overlap);
int len = num_x*num_y;
asfPrintStatus("Number of tiles is %dx%d\n", num_x, num_y);
offset_point_t *matches = MALLOC(sizeof(offset_point_t)*len);
int ii, jj, kk=0, nvalid=0;
for (ii=0; ii<num_y; ++ii) {
int tile_y = ii*(size - overlap);
if (tile_y + size > nl) {
if (ii != num_y - 1)
asfPrintError("Bad tile_y: %d %d %d %d %d\n", ii, num_y, tile_y, size, nl);
tile_y = nl - size;
}
for (jj=0; jj<num_x; ++jj) {
int tile_x = jj*(size - overlap);
if (tile_x + size > ns) {
if (jj != num_x - 1)
asfPrintError("Bad tile_x: %d %d %d %d %d\n", jj, num_x, tile_x, size, ns);
tile_x = ns - size;
}
//asfPrintStatus("Matching tile starting at (L,S) (%d,%d)\n", tile_y, tile_x);
char trim_append[64];
sprintf(trim_append, "_chip_%05d_%05d", tile_y, tile_x);
char *trim_chip1 = appendToBasename(inFile1, trim_append);
char *trim_chip2 = appendToBasename(inFile2, trim_append);
trim(inFile1, trim_chip1, (long long)tile_x, (long long)tile_y, lsz, lsz);
trim(inFile2, trim_chip2, (long long)tile_x, (long long)tile_y, lsz, lsz);
//char smooth_append[64];
//sprintf(smooth_append, "_smooth_chip_%05d_%05d", tile_x, tile_y);
//char *smooth_chip1 = appendToBasename(inFile1, smooth_append);
//smooth(trim_chip1, smooth_chip1, 3, EDGE_TRUNCATE);
float dx, dy, cert;
int ok = fftMatchBF(trim_chip1, trim_chip2, &dx, &dy, &cert, tol);
matches[kk].x_pos = tile_x;
matches[kk].y_pos = tile_y;
matches[kk].cert = cert;
matches[kk].x_offset = dx;
matches[kk].y_offset = dy;
matches[kk].valid = ok && cert>tol;
asfPrintStatus("%s: %5d %5d dx=%7.3f, dy=%7.3f, cert=%5.3f\n",
matches[kk].valid?"GOOD":"BAD ", tile_y, tile_x, dx, dy, cert);
if (matches[kk].valid) ++nvalid;
++kk;
//printf("%4.1f ", dx);
removeImgAndMeta(trim_chip1);
FREE(trim_chip1);
removeImgAndMeta(trim_chip2);
FREE(trim_chip2);
//unlink(smooth_chip1);
//FREE(smooth_chip1);
}
//printf("\n");
}
//print_matches(matches, num_x, num_y, stdout);
asfPrintStatus("Removing grid offset outliers.\n");
asfPrintStatus("Starting with %d offsets.\n", nvalid);
int removed, iter=0;
do {
removed = remove_outliers(matches, len);
if (removed > 0)
asfPrintStatus("Iteration %d: Removed %d outliers\n", ++iter, removed);
}
while (removed > 0);
asfPrintStatus("Finished removing outliers.\n");
if (gridFile) {
FILE *offset_fp = FOPEN(gridFile,"w");
print_matches(matches, num_x, num_y, offset_fp);
FCLOSE(offset_fp);
}
char *name = appendExt(inFile1, ".offsets.txt");
FILE *fp = FOPEN(name, "w");
int valid_points = 0;
for (ii=0; ii<len; ++ii) {
if (matches[ii].valid) {
++valid_points;
if (fp) {
fprintf(fp, "%5d %5d %14.5f %14.5f %14.5f\n",
matches[ii].x_pos, matches[ii].y_pos,
matches[ii].x_pos + matches[ii].x_offset,
matches[ii].y_pos + matches[ii].y_offset,
matches[ii].cert);
}
}
}
if (valid_points < 1) {
asfPrintStatus("Too few points for a good match.\n");
*avgLocX = 0;
*avgLocY = 0;
*certainty = 0;
}
else {
*avgLocX = 0;
*avgLocY = 0;
*certainty = 0;
int n = 0;
for (ii=0; ii<len; ++ii) {
if (matches[ii].valid) {
*avgLocX += matches[ii].x_offset;
*avgLocY += matches[ii].y_offset;
*certainty += matches[ii].cert;
++n;
}
}
*avgLocX /= (float)n;
*avgLocY /= (float)n;
//*certainty = (float)n / (float)len;
*certainty /= (float)n;
}
asfPrintStatus("Found %d offsets.\n", valid_points);
asfPrintStatus("Average tile offset: dx=%f, dy=%f, cert=%f\n",
*avgLocX, *avgLocY, *certainty);
meta_free(meta1);
meta_free(meta2);
FCLOSE(fp);
asfPrintStatus("Generated grid match file: %s\n", name);
FREE(matches);
FREE(name);
return (0);
}
int main (int argc, char *argv[])
{
Visualizer vs;
vs.viewer->removeAllPointClouds();
vs.viewer->removeCoordinateSystem();
vs.viewer->setBackgroundColor(0,0,0);
PointCloudPtr_RGB cloud(new PointCloud_RGB);
NormalCloudTPtr normals(new NormalCloudT);
//loadPointCloud_ply("data/scene0.ply", cloud);
//loadPointCloud_ply("data/scene1.ply", cloud);
//loadPointCloud_ply("data/big_table_after.ply", cloud);
loadPointCloud_ply("data/two_tables.ply", cloud);
//loadPointCloud_ply("data/hui_room.ply", cloud);
/******************detect floor and wall************************/
MyPointCloud_RGB floor_cloud;
pcl::ModelCoefficients floor_coefficients;
MyPointCloud floor_rect_cloud;
vector<MyPointCloud_RGB> wall_clouds;
std::vector<MyPointCloud> wall_rect_clouds;
PointCloudPtr_RGB remained_cloud(new PointCloud_RGB);
detect_floor_and_walls(cloud, floor_cloud, floor_coefficients, floor_rect_cloud, wall_clouds, wall_rect_clouds, remained_cloud);
if(floor_cloud.mypoints.size()>0){
Eigen::Matrix4f matrix_transform;
Eigen::Matrix4f matrix_translation_r;
Eigen::Matrix4f matrix_transform_r;
getTemTransformMatrix(floor_coefficients, floor_rect_cloud, matrix_transform, matrix_translation_r, matrix_transform_r);
PointCloudPtr_RGB filter_remained_cloud(new PointCloud_RGB);
remove_outliers(remained_cloud, floor_rect_cloud, wall_rect_clouds, matrix_transform, matrix_translation_r, matrix_transform_r, filter_remained_cloud, vs);
//vs.viewer->addPointCloud(filter_remained_cloud,"filter_remained_cloud");
PointCloudPtr_RGB new_remained_cloud(new PointCloud_RGB);
PointCloud_RGB ct;
pcl::copyPointCloud(*filter_remained_cloud,ct);
pcl::transformPointCloud (ct, *new_remained_cloud, matrix_transform);
/******************pre-segment scene************************/
std::vector<MyPointCloud> sum_support_clouds;
std::vector<MyPointCloud> sum_separation_rect_clouds;
pre_segment_scene(new_remained_cloud, sum_support_clouds, sum_separation_rect_clouds);
/******************segment scene************************/
vector<MyPointCloud> clustering_cloud;
segment_scene(new_remained_cloud, sum_support_clouds, sum_separation_rect_clouds, clustering_cloud, vs);
for(int i = 0; i < clustering_cloud.size(); i++){
PointCloudPtr_RGB clustering_color_cloud(new PointCloud_RGB);
MyPointCloud mpc=clustering_cloud.at(i);
for(int j = 0;j < mpc.mypoints.size(); j++){
Point_RGB pt;
pt.x=mpc.mypoints.at(j).x;
pt.y=mpc.mypoints.at(j).y;
pt.z=mpc.mypoints.at(j).z;
pt.r=new_color_table[i%30][0];
pt.g=new_color_table[i%30][1];
pt.b=new_color_table[i%30][2];
clustering_color_cloud->push_back(pt);
}
std::stringstream st;
st<<"clustering_color_cloud"<<i;
std::string id_str=st.str();
vs.viewer->addPointCloud(clustering_color_cloud,id_str);
}
vs.show();
}
return 0;
}
