BIN_LIST sort_rms_binning(double *tumor, int n_tmor,double *normal, int n_nml,int bin_size, int *nbins,int w, double quantile, double multple,FILE *outlier,char *tumor_file,char *normal_file) { int *tumor_1bpbin,num_tum_1bp, *normal_1bpbin,num_norm_1bp; BIN_LIST bins; qsort(tumor,n_tmor,sizeof(double),cmpdouble); fprintf(stderr,"sorted %d case reads\n",n_tmor); qsort(normal,n_nml,sizeof(double),cmpdouble); fprintf(stderr,"sorted %d control reads.\n",n_nml); /*aggregate the reads; */ tumor_1bpbin = aggregate(tumor,n_tmor,&num_tum_1bp); free(tumor); /*If invoke in R, it's maybe better not free tumor here*/ normal_1bpbin = aggregate(normal,n_nml,&num_norm_1bp); free(normal); /*If invoke in R, it's maybe better not free normal here*/ /*remove the singular points*/ if(outlier!=NULL) {fprintf(outlier,"#case\n#%s\npos\tcount\tquantile_in_windonw\n",tumor_file);} singularity_rm(tumor_1bpbin, num_tum_1bp, w, quantile, multple, outlier); if(outlier!=NULL) fprintf(outlier,"#control\n#%s\npos\tcount\tquantile_in_windonw\n",normal_file); singularity_rm(normal_1bpbin, num_norm_1bp, w, quantile, multple,outlier); /*bin the processed reads*/ bins = binning(tumor_1bpbin,num_tum_1bp,normal_1bpbin, num_norm_1bp,bin_size); free(tumor_1bpbin); free(normal_1bpbin); return bins; }
TH1* bookMassHistogram(const std::string& histogramName) { double xMin = 1.e+1; double xMax = 1.e+3; double logBinWidth = 1.025; int numBins = TMath::Log(xMax/xMin)/TMath::Log(logBinWidth); TArrayF binning(numBins + 1); double x = xMin; for ( int iBin = 0; iBin <= numBins; ++iBin ) { binning[iBin] = x; //std::cout << "binning[" << iBin << "] = " << binning[iBin] << std::endl; x *= logBinWidth; } TH1* histogram = new TH1D(histogramName.data(), histogramName.data(), numBins, binning.GetArray()); return histogram; }
/** * \brief Main command interpreter function * * This operator analyzes the command arguments and calls the appropriate * subcommand method. */ void guidercommand::operator()(const std::string& /* command */, const std::vector<std::string>& arguments) { if (arguments.size() < 2) { throw command_error("guider command requires more " "arguments"); } std::string guiderid = arguments[0]; std::string subcommand = arguments[1]; debug(LOG_DEBUG, DEBUG_LOG, 0, "guiderid: %s", guiderid.c_str()); Guiders guiders; GuiderWrapper guider = guiders.byname(guiderid); if (subcommand == "info") { info(guider, arguments); return; } if (subcommand == "exposure") { exposure(guider, arguments); return; } if (subcommand == "exposuretime") { exposuretime(guider, arguments); return; } if (subcommand == "binning") { binning(guider, arguments); return; } if (subcommand == "size") { size(guider, arguments); return; } if (subcommand == "offset") { offset(guider, arguments); return; } if (subcommand == "star") { star(guider, arguments); return; } if (subcommand == "calibration") { calibration(guider, arguments); return; } if (subcommand == "start") { start(guider, arguments); return; } if (subcommand == "stop") { stop(guider, arguments); return; } if (subcommand == "wait") { wait(guider, arguments); return; } if (subcommand == "image") { image(guider, arguments); return; } }
cv::Mat HOGFeatures(cv::Mat &image){ int width = image.cols; int height = image.rows; cv::Mat dx(height, width, CV_16SC2); cv::Mat mag_ang(height, width, CV_32FC2); cv::Mat bins(height, width, CV_8UC1); for(int row = 0; row < image.rows; row++) { // get a pointer to the first pixel in the current row unsigned char* rowPtr = image.ptr<unsigned char>(row); unsigned char* rowPtrM = NULL; unsigned char* rowPtrP = NULL; if(row != 0){ rowPtrM = image.ptr<unsigned char>(row-1); } if(row != image.rows-1){ rowPtrP = image.ptr<unsigned char>(row+1); } float* mag_angRowPtr = mag_ang.ptr<float>(row); short* derivRowPtr = dx.ptr<short>(row); unsigned char* binPtr = bins.ptr<unsigned char>(row); for(int col = 0; col < image.cols; col++) { float maxMag = 0; float maxAng = 0; int maxDx = 0; int maxDy = 0; int *xDerivs = new int[image.channels()]; int *yDerivs = new int[image.channels()]; float *magChannel = new float[image.channels()]; float *angChannel = new float[image.channels()]; for(int ch = 0; ch < image.channels(); ch++) { // calculate dx's per pixel and channel if((col == 0) || (col == image.cols-1)){ xDerivs[ch] = 0; } else{ xDerivs[ch] = rowPtr[(col + 1) * image.channels() + ch] - rowPtr[(col - 1) * image.channels() + ch]; } // calculate dy's per pixel and channel if((row == 0) || (row == image.rows-1)){ yDerivs[ch] = 0; } else{ yDerivs[ch] = rowPtrP[(col) * image.channels() + ch] - rowPtrM[(col) * image.channels() + ch]; } // calculate magnitudes and angles if((col == 0) || (col == image.cols-1) || (row == 0) || (row == image.rows-1)){ magChannel[ch] = 0; angChannel[ch] = 0; } else{ magChannel[ch] = sqrt((float) xDerivs[ch] * xDerivs[ch] + yDerivs[ch] * yDerivs[ch]); angChannel[ch] = (!xDerivs[ch] ? 0 : atan((float) yDerivs[ch] / xDerivs[ch])); } if(magChannel[ch] > maxMag){ maxMag = magChannel[ch]; maxAng = angChannel[ch]; maxDx = xDerivs[ch]; maxDy = yDerivs[ch]; } } //insert max magnitude, angle, and derivatives mag_angRowPtr[col * 2] = maxMag; mag_angRowPtr[col * 2 + 1] = maxAng; derivRowPtr[col * 2] = maxDx; derivRowPtr[col * 2 + 1] = maxDy; // Part 2: Partitioning pixels into bins binPtr[col] = binning(maxDx, maxDy); delete [] xDerivs; delete [] yDerivs; delete [] magChannel; delete [] angChannel; } } // Part 3: Spatial Aggregation int bin_size = 8; float cellWidthF = width / ((float) bin_size); float cellHeightF = height / ((float) bin_size); int cellWidth = (int) std::floor(cellWidthF + 0.5); int cellHeight = (int) std::floor(cellHeightF + 0.5); cv::Mat cells = cv::Mat(cellHeight, cellWidth, cv::DataType< cv::Vec<double, 32> >::type); for(int row = 0; row < image.rows; row++) { unsigned char* binPtr = bins.ptr<unsigned char>(row); float* magPtr = mag_ang.ptr<float>(row); for(int col = 0; col < image.cols; col++){ // calculate the location of where the pixel falls in cell space double xCellCoord = (col + 0.5) / bin_size - 0.5; double yCellCoord = (row + 0.5) / bin_size - 0.5; // calculate the index of the lower index cell (closest to (0,0)) that the pixel contributes to int xCellIdx = (int) std::floor(xCellCoord); int yCellIdx = (int) std::floor(yCellCoord); if(xCellIdx < 0){ xCellIdx = 0; } if(yCellIdx < 0){ yCellIdx = 0; } // calculate the fractions that this pixel contributes to each cell double fracLowerX = xCellCoord - xCellIdx; double fracLowerY = yCellCoord - yCellIdx; double fracUpperX = 1.0 - fracLowerX; double fracUpperY = 1.0 - fracLowerY; float magnitude = magPtr[col * 2]; int orientationBin = binPtr[col]; // add the contributions of this pixel to the lower cell if((xCellIdx < cellWidth) && (yCellIdx < cellHeight)){ cv::Vec<double, 32> *cellPtr = cells.ptr< cv::Vec<double, 32> >(yCellIdx); cellPtr[xCellIdx][orientationBin] += fracLowerX * fracLowerY * magnitude; } if((xCellIdx < cellWidth) && (yCellIdx + 1 < cellHeight)){ cv::Vec<double, 32> *cellPtr = cells.ptr< cv::Vec<double, 32> >(yCellIdx+1); cellPtr[xCellIdx][orientationBin] += fracUpperY * fracLowerX * magnitude; } if((xCellIdx + 1 < cellWidth) && (yCellIdx < cellHeight)){ cv::Vec<double, 32> *cellPtr = cells.ptr< cv::Vec<double, 32> >(yCellIdx); cellPtr[xCellIdx + 1][orientationBin] += fracLowerY * fracUpperX * magnitude; } if((xCellIdx + 1 < cellWidth) && (yCellIdx + 1 < cellHeight)){ cv::Vec<double, 32> *cellPtr = cells.ptr< cv::Vec<double, 32> >(yCellIdx+1); cellPtr[xCellIdx+1][orientationBin] += fracUpperX * fracUpperY * magnitude; } } } // Part 4 cv::Mat cellEnergy(cellHeight, cellWidth, CV_32SC1); for(int row = 0; row < cellHeight; row++) { int* cellEnergyPtr = cellEnergy.ptr<int>(row); cv::Vec<double, 32> *cellPtr = cells.ptr< cv::Vec<double, 32> >(row); for(int col = 0; col < cellWidth; col++) { cv::Vec<double, 32> &thisCell = cellPtr[col]; // Compute undirected magnitudes thisCell[18] = thisCell[0] + thisCell[9]; thisCell[19] = thisCell[1] + thisCell[10]; thisCell[20] = thisCell[2] + thisCell[11]; thisCell[21] = thisCell[3] + thisCell[12]; thisCell[22] = thisCell[4] + thisCell[13]; thisCell[23] = thisCell[5] + thisCell[14]; thisCell[24] = thisCell[6] + thisCell[15]; thisCell[25] = thisCell[7] + thisCell[16]; thisCell[26] = thisCell[8] + thisCell[17]; thisCell[27] = 0; thisCell[28] = 0; thisCell[29] = 0; thisCell[30] = 0; thisCell[31] = 0; // Compute energy of each cell cellEnergyPtr[col] = thisCell[18] * thisCell[18] + thisCell[19] * thisCell[19] + thisCell[20] * thisCell[20] + thisCell[21] * thisCell[21] + thisCell[22] * thisCell[22] + thisCell[23] * thisCell[23] + thisCell[24] * thisCell[24] + thisCell[25] * thisCell[25] + thisCell[26] * thisCell[26]; } } cv::Mat blockEnergy(cellHeight-1, cellWidth-1, CV_32SC1); for(int row = 0; row < cellHeight-1; row++) { int* cellEnergyPtr = cellEnergy.ptr<int>(row); int* cellEnergyP1Ptr = cellEnergy.ptr<int>(row+1); int* blockEnergyPtr = blockEnergy.ptr<int>(row); for(int col = 0; col < cellWidth-1; col++) { // Compute block energy blockEnergyPtr[col] = cellEnergyPtr[col] + cellEnergyPtr[col+1] + cellEnergyP1Ptr[col] + cellEnergyP1Ptr[col+1]; } } //Compute a normalized feature vector // cv::Mat featureVector = cv::Mat(cellHeight, cellWidth, cv::DataType< // cv::Vec<double, 32> >::type); for(int row = 0; row < cellHeight; row++) { if(row != 0 && row != cellHeight-1){ int* blockEnergyPtr = blockEnergy.ptr<int>(row); int* blockEnergyM1Ptr = blockEnergy.ptr<int>(row-1); cv::Vec<double, 32> *cellPtr = cells.ptr< cv::Vec<double, 32> >(row); for(int col = 0; col < cellWidth; col++) { if(col != 0 && col != cellWidth-1){ int norm3 = blockEnergyM1Ptr[col - 1]; int norm1 = blockEnergyM1Ptr[col]; int norm2 = blockEnergyPtr[col - 1]; int norm0 = blockEnergyPtr[col]; cv::Vec<double, 32> &thisCell = cellPtr[col]; for(int i=0; i<18; i++){ double oldValue = thisCell[i]; thisCell[i] = (1/2) * (std::min(oldValue * norm0, THRESHOLD) + std::min(oldValue * norm1, THRESHOLD) + std::min(oldValue * norm2, THRESHOLD) + std::min(oldValue * norm3, THRESHOLD)); thisCell[28] += std::min(oldValue * norm0, THRESHOLD); thisCell[29] += std::min(oldValue * norm1, THRESHOLD); thisCell[30] += std::min(oldValue * norm2, THRESHOLD); thisCell[31] += std::min(oldValue * norm3, THRESHOLD); } for(int i=18; i<27; i++){ double oldValue = thisCell[i]; thisCell[i] = (1/2) * (std::min(oldValue * norm0, THRESHOLD) + std::min(oldValue * norm1, THRESHOLD) + std::min(oldValue * norm2, THRESHOLD) + std::min(oldValue * norm3, THRESHOLD)); } for(int i=27; i<=30; i++){ // yes this is a magic number, // no i don't know why it is important thisCell[i] *= 0.2357; } } } } } return cv::Mat(cells, cv::Rect(1, 1, cells.cols - 2, cells.rows - 2)); }
int main(int argc, char **argv) { struct GModule *module; struct Option *voutput_opt, *routput_opt, *color_output_opt, *ply_opt, *zrange_opt, *trim_opt, *rotate_Z_opt, *smooth_radius_opt, *region_opt, *raster_opt, *zexag_opt, *resolution_opt, *method_opt, *calib_matrix_opt, *numscan_opt, *trim_tolerance_opt, *contours_map, *contours_step_opt, *draw_opt, *draw_vector_opt, *draw_threshold_opt; struct Flag *loop_flag, *calib_flag, *equalize_flag; struct Map_info Map; struct line_pnts *Points; struct line_cats *Cats; int cat = 1; G_gisinit(argv[0]); module = G_define_module(); G_add_keyword(_("vector")); G_add_keyword(_("scan")); G_add_keyword(_("points")); module->label = _("Imports a point cloud from Kinect v2"); module->description = _("Imports a point cloud from Kinect v2"); routput_opt = G_define_standard_option(G_OPT_R_OUTPUT); routput_opt->guisection = _("Output"); routput_opt->required = NO; resolution_opt = G_define_option(); resolution_opt->key = "resolution"; resolution_opt->type = TYPE_DOUBLE; resolution_opt->required = NO; resolution_opt->answer = "0.002"; resolution_opt->label = _("Raster resolution"); resolution_opt->description = _("Recommended values between 0.001-0.003"); resolution_opt->guisection = _("Output"); color_output_opt = G_define_standard_option(G_OPT_R_BASENAME_OUTPUT); color_output_opt->key = "color_output"; color_output_opt->description = _("Basename for color output"); color_output_opt->guisection = _("Output"); color_output_opt->required = NO; voutput_opt = G_define_standard_option(G_OPT_V_OUTPUT); voutput_opt->required = NO; voutput_opt->key = "vector"; voutput_opt->guisection = _("Output"); ply_opt = G_define_standard_option(G_OPT_F_OUTPUT); ply_opt->required = NO; ply_opt->key = "ply"; ply_opt->description = _("Name of output binary PLY file"); ply_opt->guisection = _("Output"); zrange_opt = G_define_option(); zrange_opt->key = "zrange"; zrange_opt->type = TYPE_DOUBLE; zrange_opt->required = NO; zrange_opt->key_desc = "min,max"; zrange_opt->label = _("Filter range for z data (min,max)"); zrange_opt->description = _("Z is distance from scanner in cm"); zrange_opt->guisection = _("Filter"); trim_opt = G_define_option(); trim_opt->key = "trim"; trim_opt->type = TYPE_DOUBLE; trim_opt->required = NO; trim_opt->key_desc = "N,S,E,W"; trim_opt->description = _("Clip box from center in cm"); trim_opt->guisection = _("Filter"); trim_tolerance_opt = G_define_option(); trim_tolerance_opt->key = "trim_tolerance"; trim_tolerance_opt->type = TYPE_DOUBLE; trim_tolerance_opt->required = NO; trim_tolerance_opt->description = _("Influences how much are model sides trimmed automatically, " " should be higher for rectangular models"); trim_tolerance_opt->label = _("Trim tolerance between 0 and 1"); trim_tolerance_opt->options = "0-1"; trim_tolerance_opt->guisection = _("Filter"); rotate_Z_opt = G_define_option(); rotate_Z_opt->key = "rotate"; rotate_Z_opt->type = TYPE_DOUBLE; rotate_Z_opt->required = NO; rotate_Z_opt->answer = "0"; rotate_Z_opt->description = _("Rotate along Z axis"); rotate_Z_opt->guisection = _("Georeferencing"); smooth_radius_opt = G_define_option(); smooth_radius_opt->key = "smooth_radius"; smooth_radius_opt->type = TYPE_DOUBLE; smooth_radius_opt->required = NO; smooth_radius_opt->label = _("Smooth radius"); smooth_radius_opt->description = _("Recommended values between 0.006-0.009"); region_opt = G_define_option(); region_opt->key = "region"; region_opt->key_desc = "name"; region_opt->required = NO; region_opt->multiple = NO; region_opt->type = TYPE_STRING; region_opt->description = _("Region of the resulting raster"); region_opt->gisprompt = "old,windows,region"; region_opt->guisection = _("Georeferencing"); raster_opt = G_define_standard_option(G_OPT_R_MAP); raster_opt->key = "raster"; raster_opt->required = NO; raster_opt->multiple = NO; raster_opt->description = _("Match resulting raster to this raster map"); raster_opt->guisection = _("Georeferencing"); zexag_opt = G_define_option(); zexag_opt->key = "zexag"; zexag_opt->type = TYPE_DOUBLE; zexag_opt->required = NO; zexag_opt->required = NO; zexag_opt->answer = "1"; zexag_opt->description = _("Vertical exaggeration"); zexag_opt->guisection = _("Georeferencing"); method_opt = G_define_option(); method_opt->key = "method"; method_opt->multiple = NO; method_opt->required = NO; method_opt->type = TYPE_STRING; method_opt->options = "interpolation,mean,min,max"; method_opt->answer = "mean"; method_opt->description = _("Surface reconstruction method"); calib_matrix_opt = G_define_option(); calib_matrix_opt->key = "calib_matrix"; calib_matrix_opt->multiple = YES; calib_matrix_opt->type = TYPE_DOUBLE; calib_matrix_opt->required = NO; calib_matrix_opt->description = _("Calibration matrix"); calib_matrix_opt->guisection = _("Calibration"); numscan_opt = G_define_option(); numscan_opt->answer = "1"; numscan_opt->key = "numscan"; numscan_opt->type = TYPE_INTEGER; numscan_opt->description = _("Number of scans to intergrate"); numscan_opt->required = NO; contours_map = G_define_standard_option(G_OPT_V_MAP); contours_map->key = "contours"; contours_map->required = NO; contours_map->description = _("Name of contour vector map"); contours_step_opt = G_define_option(); contours_step_opt->key = "contours_step"; contours_step_opt->description = _("Increment between contour levels"); contours_step_opt->type = TYPE_DOUBLE; contours_step_opt->required = NO; equalize_flag = G_define_flag(); equalize_flag->key = 'e'; equalize_flag->description = _("Histogram equalized color table"); loop_flag = G_define_flag(); loop_flag->key = 'l'; loop_flag->description = _("Keep scanning in a loop"); calib_flag = G_define_flag(); calib_flag->key = 'c'; calib_flag->description = _("Calibrate"); calib_flag->guisection = _("Calibration"); draw_opt = G_define_option(); draw_opt->key = "draw"; draw_opt->description = _("Draw with laser pointer"); draw_opt->type = TYPE_STRING; draw_opt->required = NO; draw_opt->options = "point,line,area"; draw_opt->answer = "point"; draw_opt->guisection = _("Drawing"); draw_threshold_opt = G_define_option(); draw_threshold_opt->key = "draw_threshold"; draw_threshold_opt->description = _("Brightness threshold for detecting laser pointer"); draw_threshold_opt->type = TYPE_INTEGER; draw_threshold_opt->required = YES; draw_threshold_opt->answer = "760"; draw_threshold_opt->guisection = _("Drawing"); draw_vector_opt = G_define_standard_option(G_OPT_V_OUTPUT); draw_vector_opt->key = "draw_output"; draw_vector_opt->guisection = _("Drawing"); draw_vector_opt->required = NO; G_option_required(calib_flag, routput_opt, voutput_opt, ply_opt, draw_vector_opt, NULL); G_option_requires(routput_opt, resolution_opt, NULL); G_option_requires(color_output_opt, resolution_opt, NULL); G_option_requires(contours_map, contours_step_opt, routput_opt, NULL); G_option_requires(equalize_flag, routput_opt, NULL); if (G_parser(argc, argv)) exit(EXIT_FAILURE); // initailization of variables double resolution = 0.002; if (resolution_opt->answer) resolution = atof(resolution_opt->answer); double smooth_radius = 0.008; if (smooth_radius_opt->answer) smooth_radius = atof(smooth_radius_opt->answer); char* routput = NULL; if (routput_opt->answer) routput = routput_opt->answer; /* parse zrange */ double zrange_min, zrange_max; if (zrange_opt->answer != NULL) { zrange_min = atof(zrange_opt->answers[0])/100; zrange_max = atof(zrange_opt->answers[1])/100; } /* parse trim */ double clip_N, clip_S, clip_E, clip_W; if (trim_opt->answer != NULL) { clip_N = atof(trim_opt->answers[0])/100; clip_S = atof(trim_opt->answers[1])/100; clip_E = atof(trim_opt->answers[2])/100; clip_W = atof(trim_opt->answers[3])/100; } double trim_tolerance; if (trim_tolerance_opt->answer) trim_tolerance = atof(trim_tolerance_opt->answer); double angle = pcl::deg2rad(atof(rotate_Z_opt->answer) + 180); double zexag = atof(zexag_opt->answer); Eigen::Matrix4f transform_matrix; if (calib_matrix_opt->answer) { transform_matrix = read_matrix(calib_matrix_opt); } char *method = method_opt->answer; int numscan = atoi(numscan_opt->answer); char *color_output = color_output_opt->answer; char *voutput = voutput_opt->answer; char *ply = ply_opt->answer; char *contours_output = contours_map->answer; double contours_step; if (contours_output) contours_step = atof(contours_step_opt->answer); bool use_equalized = false; if (equalize_flag->answer) use_equalized = true; // drawing int vect_type; get_draw_type(draw_opt->answer, vect_type); int draw_threshold = atoi(draw_threshold_opt->answer); char* draw_output = NULL; if (draw_vector_opt->answer) draw_output = draw_vector_opt->answer; std::vector<double> draw_x; std::vector<double> draw_y; std::vector<double> draw_z; bool drawing = false; unsigned int last_detected_loop_count = 1e6; struct Map_info Map_draw; struct line_pnts *Points_draw; struct line_cats *Cats_draw; Points_draw = Vect_new_line_struct(); Cats_draw = Vect_new_cats_struct(); Points = Vect_new_line_struct(); Cats = Vect_new_cats_struct(); pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZRGB>(512, 424)); pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud_filtered_pass (new pcl::PointCloud<pcl::PointXYZRGB>(512, 424)); struct bound_box bbox; struct Cell_head cellhd, window; double offset, scale; bool region3D = false; bool paused = false; update_input_region(raster_opt->answer, region_opt->answer, window, offset, region3D); K2G k2g(OPENGL); k2g.getCloud(); cloud->sensor_orientation_.w() = 0.0; cloud->sensor_orientation_.x() = 1.0; cloud->sensor_orientation_.y() = 0.0; cloud->sensor_orientation_.z() = 0.0; int j = 0; // get terminating signals signal(SIGTERM, terminate); signal(SIGINT, terminate); signal(SIGUSR1, signal_read_new_input); while (j < 1) { if (signaled == 1) { break; } if (signal_new_input == 1) { signal_new_input = 0; read_new_input(routput, zrange_min, zrange_max, clip_N, clip_S, clip_E, clip_W, trim_tolerance, angle, zexag, method, numscan, smooth_radius, resolution, use_equalized, window, offset, region3D, color_output, voutput, ply, contours_output, contours_step, vect_type, draw_threshold, draw_output, paused); } cloud = k2g.getCloud(); if (paused) { continue; } if (!drawing) { for (int s = 0; s < numscan - 1; s++) *(cloud) += *(k2g.getCloud()); } // remove invalid points std::vector<int> index_nans; pcl::removeNaNFromPointCloud(*cloud, *cloud, index_nans); // calibration if(calib_flag->answer) { calibrate(cloud); j++; continue; } // rotation of the point cloud based on calibration if (calib_matrix_opt->answer) { rotate_with_matrix(cloud, transform_matrix); } // trim Z if (zrange_opt->answer != NULL) { trim_Z(cloud, zrange_min, zrange_max); } // rotation Z rotate_Z(cloud, angle); // specify bounding box from center if (trim_opt->answer != NULL) { clipNSEW(cloud, clip_N, clip_S, clip_E, clip_W); } // drawing if (draw_output) { int maxbright = 0; int maxbright_idx = 0; for (int i=0; i < cloud->points.size(); i++) { Eigen::Vector3i rgbv = cloud->points[i].getRGBVector3i(); int sum = rgbv[0] + rgbv[1] + rgbv[2]; if (sum > maxbright) { maxbright = sum; maxbright_idx = i; } } if (maxbright >= draw_threshold) { drawing = true; draw_x.push_back(cloud->points[maxbright_idx].x); draw_y.push_back(cloud->points[maxbright_idx].y); draw_z.push_back(cloud->points[maxbright_idx].z); last_detected_loop_count = 0; continue; } else { last_detected_loop_count++; if (last_detected_loop_count <= 2) { continue; } } } pcl::StatisticalOutlierRemoval<pcl::PointXYZRGB> sor; sor.setInputCloud(cloud); sor.setMeanK(20); sor.setStddevMulThresh(0.5); sor.filter(*cloud_filtered_pass); cloud_filtered_pass.swap (cloud); if (trim_tolerance_opt->answer != NULL) { double autoclip_N, autoclip_S, autoclip_E, autoclip_W; autotrim(cloud, autoclip_N, autoclip_S, autoclip_E, autoclip_W, trim_tolerance); if (autoclip_E > 0 || autoclip_N > 0 || autoclip_S > 0 || autoclip_W > 0) trimNSEW(cloud, autoclip_N, autoclip_S, autoclip_E, autoclip_W); } if (drawing) { // get Z scaling getMinMax(*cloud, bbox); if ((vect_type == GV_AREA && draw_x.size() > 2) || (vect_type == GV_LINE && draw_x.size() > 1) || (vect_type == GV_POINT && draw_x.size() > 0)) { save_vector(draw_output, Map_draw, Points_draw, Cats_draw, bbox, window, draw_x, draw_y, draw_z, vect_type, offset, zexag); } else G_warning(_("Tolopogically incorrect vector feature")); drawing = false; draw_x.clear(); draw_y.clear(); draw_z.clear(); last_detected_loop_count = 1e6; } if (voutput|| routput || ply || color_output) { if (smooth_radius_opt->answer) smooth(cloud, smooth_radius); // get Z scaling getMinMax(*cloud, bbox); scale = ((window.north - window.south) / (bbox.N - bbox.S) + (window.east - window.west) / (bbox.E - bbox.W)) / 2; } // write to vector if (voutput|| (routput && strcmp(method, "interpolation") == 0)) { double z; if (voutput) { if (Vect_open_new(&Map, voutput, WITH_Z) < 0) G_fatal_error(_("Unable to create temporary vector map <%s>"), voutput); } else { if (Vect_open_tmp_new(&Map, routput, WITH_Z) < 0) G_fatal_error(_("Unable to create temporary vector map <%s>"), routput); } for (int i=0; i < cloud->points.size(); i++) { Vect_reset_line(Points); Vect_reset_cats(Cats); if (region3D) z = (cloud->points[i].z + zrange_max) * scale / zexag + offset; else z = (cloud->points[i].z - bbox.B) * scale / zexag + offset; Vect_append_point(Points, cloud->points[i].x, cloud->points[i].y, z); Vect_cat_set(Cats, 1, cat); Vect_write_line(&Map, GV_POINT, Points, Cats); } if (strcmp(method, "interpolation") == 0) { // interpolate Vect_rewind(&Map); interpolate(&Map, routput, 20, 2, 50, 40, -1, &bbox, resolution); } Vect_close(&Map); } if (routput || color_output) { if (routput) { if (strcmp(method, "interpolation") != 0) { binning(cloud, routput, &bbox, resolution, scale, zexag, region3D ? -zrange_max : bbox.B, offset, method); } Rast_get_cellhd(routput, "", &cellhd); } if (color_output) { binning_color(cloud, color_output, &bbox, resolution); Rast_get_cellhd(get_color_name(color_output, "r"), "", &cellhd); } // georeference horizontally window.rows = cellhd.rows; window.cols = cellhd.cols; G_adjust_Cell_head(&window, 1, 1); cellhd.north = window.north; cellhd.south = window.south; cellhd.east = window.east; cellhd.west = window.west; if (routput) Rast_put_cellhd(routput, &cellhd); if (color_output) { char* output_r = get_color_name(color_output, "r"); char* output_g = get_color_name(color_output, "g"); char* output_b = get_color_name(color_output, "b"); Rast_put_cellhd(output_r, &cellhd); Rast_put_cellhd(output_g, &cellhd); Rast_put_cellhd(output_b, &cellhd); } set_default_color(routput); if (contours_output) { contours(routput, contours_output, atof(contours_step_opt->answer)); } if (use_equalized) { equalized(routput); } } // write to PLY if (ply) { pcl::PLYWriter writer; for (int i=0; i < cloud->points.size(); i++) { if (region3D) cloud->points[i].z = (cloud->points[i].z + zrange_max) * scale / zexag + offset; else cloud->points[i].z = (cloud->points[i].z - bbox.B) * scale / zexag + offset; cloud->points[i].x = (cloud->points[i].x - bbox.W) * scale + window.west; cloud->points[i].y = (cloud->points[i].y - bbox.S) * scale + window.south; } writer.write<pcl::PointXYZRGB>(ply, *cloud, true, true); } if (!loop_flag->answer) j++; } k2g.shutDown(); return EXIT_SUCCESS; }