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;
}
