static void output_sep(struct grep_opt *opt, char sign)
{
if (opt->null_following_name)
opt->output(opt, "\0", 1);
else
output_color(opt, &sign, 1, opt->color_sep);
}
static void show_line(struct grep_opt *opt, char *bol, char *eol,
const char *name, unsigned lno, char sign)
{
int rest = eol - bol;
char *line_color = NULL;
if (opt->pre_context || opt->post_context) {
if (opt->last_shown == 0) {
if (opt->show_hunk_mark) {
output_color(opt, "--", 2, opt->color_sep);
opt->output(opt, "\n", 1);
} else
opt->show_hunk_mark = 1;
} else if (lno > opt->last_shown + 1) {
output_color(opt, "--", 2, opt->color_sep);
opt->output(opt, "\n", 1);
}
}
opt->last_shown = lno;
if (opt->pathname) {
output_color(opt, name, strlen(name), opt->color_filename);
output_sep(opt, sign);
}
if (opt->linenum) {
char buf[32];
snprintf(buf, sizeof(buf), "%d", lno);
output_color(opt, buf, strlen(buf), opt->color_lineno);
output_sep(opt, sign);
}
if (opt->color) {
regmatch_t match;
enum grep_context ctx = GREP_CONTEXT_BODY;
int ch = *eol;
int eflags = 0;
if (sign == ':')
line_color = opt->color_selected;
else if (sign == '-')
line_color = opt->color_context;
else if (sign == '=')
line_color = opt->color_function;
*eol = '\0';
while (next_match(opt, bol, eol, ctx, &match, eflags)) {
if (match.rm_so == match.rm_eo)
break;
output_color(opt, bol, match.rm_so, line_color);
output_color(opt, bol + match.rm_so,
match.rm_eo - match.rm_so,
opt->color_match);
bol += match.rm_eo;
rest -= match.rm_eo;
eflags = REG_NOTBOL;
}
*eol = ch;
}
output_color(opt, bol, rest, line_color);
opt->output(opt, "\n", 1);
}
static int grep_buffer_1(struct grep_opt *opt, const char *name,
char *buf, unsigned long size, int collect_hits)
{
char *bol = buf;
unsigned long left = size;
unsigned lno = 1;
unsigned last_hit = 0;
int binary_match_only = 0;
unsigned count = 0;
int try_lookahead = 0;
enum grep_context ctx = GREP_CONTEXT_HEAD;
xdemitconf_t xecfg;
opt->last_shown = 0;
if (!opt->output)
opt->output = std_output;
if (buffer_is_binary(buf, size)) {
switch (opt->binary) {
case GREP_BINARY_DEFAULT:
binary_match_only = 1;
break;
case GREP_BINARY_NOMATCH:
return 0; /* Assume unmatch */
break;
default:
break;
}
}
memset(&xecfg, 0, sizeof(xecfg));
if (opt->funcname && !opt->unmatch_name_only && !opt->status_only &&
!opt->name_only && !binary_match_only && !collect_hits) {
struct userdiff_driver *drv = userdiff_find_by_path(name);
if (drv && drv->funcname.pattern) {
const struct userdiff_funcname *pe = &drv->funcname;
xdiff_set_find_func(&xecfg, pe->pattern, pe->cflags);
opt->priv = &xecfg;
}
}
try_lookahead = should_lookahead(opt);
while (left) {
char *eol, ch;
int hit;
/*
* look_ahead() skips quicly to the line that possibly
* has the next hit; don't call it if we need to do
* something more than just skipping the current line
* in response to an unmatch for the current line. E.g.
* inside a post-context window, we will show the current
* line as a context around the previous hit when it
* doesn't hit.
*/
if (try_lookahead
&& !(last_hit
&& lno <= last_hit + opt->post_context)
&& look_ahead(opt, &left, &lno, &bol))
break;
eol = end_of_line(bol, &left);
ch = *eol;
*eol = 0;
if ((ctx == GREP_CONTEXT_HEAD) && (eol == bol))
ctx = GREP_CONTEXT_BODY;
hit = match_line(opt, bol, eol, ctx, collect_hits);
*eol = ch;
if (collect_hits)
goto next_line;
/* "grep -v -e foo -e bla" should list lines
* that do not have either, so inversion should
* be done outside.
*/
if (opt->invert)
hit = !hit;
if (opt->unmatch_name_only) {
if (hit)
return 0;
goto next_line;
}
if (hit) {
count++;
if (opt->status_only)
return 1;
if (binary_match_only) {
opt->output(opt, "Binary file ", 12);
output_color(opt, name, strlen(name),
opt->color_filename);
opt->output(opt, " matches\n", 9);
return 1;
}
if (opt->name_only) {
show_name(opt, name);
return 1;
}
/* Hit at this line. If we haven't shown the
* pre-context lines, we would need to show them.
* When asked to do "count", this still show
* the context which is nonsense, but the user
* deserves to get that ;-).
*/
if (opt->pre_context)
show_pre_context(opt, name, buf, bol, lno);
else if (opt->funcname)
show_funcname_line(opt, name, buf, bol, lno);
if (!opt->count)
show_line(opt, bol, eol, name, lno, ':');
last_hit = lno;
}
else if (last_hit &&
lno <= last_hit + opt->post_context) {
/* If the last hit is within the post context,
* we need to show this line.
*/
show_line(opt, bol, eol, name, lno, '-');
}
next_line:
bol = eol + 1;
if (!left)
break;
left--;
lno++;
}
if (collect_hits)
return 0;
if (opt->status_only)
return 0;
if (opt->unmatch_name_only) {
/* We did not see any hit, so we want to show this */
show_name(opt, name);
return 1;
}
xdiff_clear_find_func(&xecfg);
opt->priv = NULL;
/* NEEDSWORK:
* The real "grep -c foo *.c" gives many "bar.c:0" lines,
* which feels mostly useless but sometimes useful. Maybe
* make it another option? For now suppress them.
*/
if (opt->count && count) {
char buf[32];
output_color(opt, name, strlen(name), opt->color_filename);
output_sep(opt, ':');
snprintf(buf, sizeof(buf), "%u\n", count);
opt->output(opt, buf, strlen(buf));
}
return !!last_hit;
}
static void show_name(struct grep_opt *opt, const char *name)
{
output_color(opt, name, strlen(name), opt->color_filename);
opt->output(opt, opt->null_following_name ? "\0" : "\n", 1);
}
static int grep_source_1(struct grep_opt *opt, struct grep_source *gs, int collect_hits)
{
char *bol;
unsigned long left;
unsigned lno = 1;
unsigned last_hit = 0;
int binary_match_only = 0;
unsigned count = 0;
int try_lookahead = 0;
int show_function = 0;
struct userdiff_driver *textconv = NULL;
enum grep_context ctx = GREP_CONTEXT_HEAD;
xdemitconf_t xecfg;
if (!opt->output)
opt->output = std_output;
if (opt->pre_context || opt->post_context || opt->file_break ||
opt->funcbody) {
/* Show hunk marks, except for the first file. */
if (opt->last_shown)
opt->show_hunk_mark = 1;
/*
* If we're using threads then we can't easily identify
* the first file. Always put hunk marks in that case
* and skip the very first one later in work_done().
*/
if (opt->output != std_output)
opt->show_hunk_mark = 1;
}
opt->last_shown = 0;
if (opt->allow_textconv) {
grep_source_load_driver(gs);
/*
* We might set up the shared textconv cache data here, which
* is not thread-safe.
*/
grep_attr_lock();
textconv = userdiff_get_textconv(gs->driver);
grep_attr_unlock();
}
/*
* We know the result of a textconv is text, so we only have to care
* about binary handling if we are not using it.
*/
if (!textconv) {
switch (opt->binary) {
case GREP_BINARY_DEFAULT:
if (grep_source_is_binary(gs))
binary_match_only = 1;
break;
case GREP_BINARY_NOMATCH:
if (grep_source_is_binary(gs))
return 0; /* Assume unmatch */
break;
case GREP_BINARY_TEXT:
break;
default:
die("bug: unknown binary handling mode");
}
}
memset(&xecfg, 0, sizeof(xecfg));
opt->priv = &xecfg;
try_lookahead = should_lookahead(opt);
if (fill_textconv_grep(textconv, gs) < 0)
return 0;
bol = gs->buf;
left = gs->size;
while (left) {
char *eol, ch;
int hit;
/*
* look_ahead() skips quickly to the line that possibly
* has the next hit; don't call it if we need to do
* something more than just skipping the current line
* in response to an unmatch for the current line. E.g.
* inside a post-context window, we will show the current
* line as a context around the previous hit when it
* doesn't hit.
*/
if (try_lookahead
&& !(last_hit
&& (show_function ||
lno <= last_hit + opt->post_context))
&& look_ahead(opt, &left, &lno, &bol))
break;
eol = end_of_line(bol, &left);
ch = *eol;
*eol = 0;
if ((ctx == GREP_CONTEXT_HEAD) && (eol == bol))
ctx = GREP_CONTEXT_BODY;
hit = match_line(opt, bol, eol, ctx, collect_hits);
*eol = ch;
if (collect_hits)
goto next_line;
/* "grep -v -e foo -e bla" should list lines
* that do not have either, so inversion should
* be done outside.
*/
if (opt->invert)
hit = !hit;
if (opt->unmatch_name_only) {
if (hit)
return 0;
goto next_line;
}
if (hit) {
count++;
if (opt->status_only)
return 1;
if (opt->name_only) {
show_name(opt, gs->name);
return 1;
}
if (opt->count)
goto next_line;
if (binary_match_only) {
opt->output(opt, "Binary file ", 12);
output_color(opt, gs->name, strlen(gs->name),
opt->color_filename);
opt->output(opt, " matches\n", 9);
return 1;
}
/* Hit at this line. If we haven't shown the
* pre-context lines, we would need to show them.
*/
if (opt->pre_context || opt->funcbody)
show_pre_context(opt, gs, bol, eol, lno);
else if (opt->funcname)
show_funcname_line(opt, gs, bol, lno);
show_line(opt, bol, eol, gs->name, lno, ':');
last_hit = lno;
if (opt->funcbody)
show_function = 1;
goto next_line;
}
if (show_function && match_funcname(opt, gs, bol, eol))
show_function = 0;
if (show_function ||
(last_hit && lno <= last_hit + opt->post_context)) {
/* If the last hit is within the post context,
* we need to show this line.
*/
show_line(opt, bol, eol, gs->name, lno, '-');
}
next_line:
bol = eol + 1;
if (!left)
break;
left--;
lno++;
}
if (collect_hits)
return 0;
if (opt->status_only)
return 0;
if (opt->unmatch_name_only) {
/* We did not see any hit, so we want to show this */
show_name(opt, gs->name);
return 1;
}
xdiff_clear_find_func(&xecfg);
opt->priv = NULL;
/* NEEDSWORK:
* The real "grep -c foo *.c" gives many "bar.c:0" lines,
* which feels mostly useless but sometimes useful. Maybe
* make it another option? For now suppress them.
*/
if (opt->count && count) {
char buf[32];
if (opt->pathname) {
output_color(opt, gs->name, strlen(gs->name),
opt->color_filename);
output_sep(opt, ':');
}
snprintf(buf, sizeof(buf), "%u\n", count);
opt->output(opt, buf, strlen(buf));
return 1;
}
return !!last_hit;
}
int main(int argc, char **argv)
{
char *descriptor;
char *curpos; /* current position in input string */
struct keyval *parser_id; /* the parser we are creating output for */
FILE *outputf;
struct keyval *rkv; /* current read key:val */
struct keyval_list *curlist;
bool do_token_check = true; /* if the check for valid tokens is done */
bool only_ident = true; /* if the only token type is ident */
bool is_generic = false;
struct keyval_list base;
struct keyval_list IDENT;
struct keyval_list IDENT_LIST;
struct keyval_list LENGTH_UNIT;
struct keyval_list URI;
struct keyval_list WRAP;
struct keyval_list NUMBER;
struct keyval_list COLOR;
if (argc < 2) {
fprintf(stderr,"Usage: %s [-o <filename>] <descriptor>\n", argv[0]);
return 1;
}
if ((argv[1][0] == '-') && (argv[1][1] == 'o')) {
if (argc != 4) {
fprintf(stderr,"Usage: %s [-o <filename>] <descriptor>\n", argv[0]);
return 1;
}
outputf = fopen(argv[2], "w");
if (outputf == NULL) {
perror("unable to open file");
}
descriptor = strdup(argv[3]);
} else {
outputf = stdout;
descriptor = strdup(argv[1]);
}
curpos = descriptor;
base.count = 0;
IDENT.count = 0;
URI.count = 0;
WRAP.count = 0;
NUMBER.count = 0;
COLOR.count = 0;
LENGTH_UNIT.count = 0;
IDENT_LIST.count = 0;
curlist = &base;
while (*curpos != 0) {
rkv = get_keyval(&curpos);
if (rkv == NULL) {
fprintf(stderr,"Token error at offset %ld\n", curpos - descriptor);
fclose(outputf);
return 2;
}
if (strcmp(rkv->key, "WRAP") == 0) {
WRAP.item[WRAP.count++] = rkv;
only_ident = false;
} else if (strcmp(rkv->key, "NUMBER") == 0) {
if (rkv->val[0] == '(') {
curlist = &NUMBER;
} else if (rkv->val[0] == ')') {
curlist = &base;
} else {
NUMBER.item[NUMBER.count++] = rkv;
}
only_ident = false;
} else if (strcmp(rkv->key, "IDENT") == 0) {
if (rkv->val[0] == '(') {
curlist = &IDENT;
} else if (rkv->val[0] == ')') {
curlist = &base;
} else if (strcmp(rkv->val, str_INHERIT) == 0) {
IDENT.item[IDENT.count++] = &ident_inherit;
}
} else if (strcmp(rkv->key, "IDENT_LIST") == 0) {
if (rkv->val[0] == '(') {
curlist = &IDENT_LIST;
} else if (rkv->val[0] == ')') {
curlist = &base;
}
} else if (strcmp(rkv->key, "LENGTH_UNIT") == 0) {
if (rkv->val[0] == '(') {
curlist = &LENGTH_UNIT;
} else if (rkv->val[0] == ')') {
curlist = &base;
}
only_ident = false;
do_token_check = false;
} else if (strcmp(rkv->key, "COLOR") == 0) {
COLOR.item[COLOR.count++] = rkv;
do_token_check = false;
only_ident = false;
} else if (strcmp(rkv->key, "URI") == 0) {
URI.item[URI.count++] = rkv;
only_ident = false;
} else if (strcmp(rkv->key, "GENERIC") == 0) {
is_generic = true;
} else {
/* just append to current list */
curlist->item[curlist->count++] = rkv;
}
}
if (base.count != 1) {
fprintf(stderr,"Incorrect base element count (got %d expected 1)\n", base.count);
fclose(outputf);
return 3;
}
/* header */
output_header(outputf, descriptor, base.item[0], is_generic);
if (WRAP.count > 0) {
output_wrap(outputf, base.item[0], &WRAP);
} else {
/* check token type is correct */
output_token_type_check(outputf, do_token_check, &IDENT, &URI, &NUMBER);
if (IDENT.count > 0)
output_ident(outputf, only_ident, base.item[0], &IDENT);
if (URI.count > 0)
output_uri(outputf, base.item[0], &URI);
if (NUMBER.count > 0)
output_number(outputf, base.item[0], &NUMBER);
/* terminal blocks, these end the ladder ie no trailing else */
if (COLOR.count > 0) {
output_color(outputf, base.item[0], &COLOR);
} else if (LENGTH_UNIT.count > 0) {
output_length_unit(outputf, base.item[0], &LENGTH_UNIT);
} else if (IDENT_LIST.count > 0) {
output_ident_list(outputf, base.item[0], &IDENT_LIST);
} else {
output_invalidcss(outputf);
}
output_footer(outputf);
}
fclose(outputf);
return 0;
}
int
main (int argc, char** argv)
{
// Loading first scan of room.
pcl::PointCloud<pcl::PointXYZ>::Ptr target_cloud (new pcl::PointCloud<pcl::PointXYZ>);
if (pcl::io::loadPCDFile<pcl::PointXYZ> ("room_scan1.pcd", *target_cloud) == -1)
{
PCL_ERROR ("Couldn't read file room_scan1.pcd \n");
return (-1);
}
std::cout << "Loaded " << target_cloud->size () << " data points from room_scan1.pcd" << std::endl;
// Loading second scan of room from new perspective.
pcl::PointCloud<pcl::PointXYZ>::Ptr input_cloud (new pcl::PointCloud<pcl::PointXYZ>);
if (pcl::io::loadPCDFile<pcl::PointXYZ> ("room_scan2.pcd", *input_cloud) == -1)
{
PCL_ERROR ("Couldn't read file room_scan2.pcd \n");
return (-1);
}
std::cout << "Loaded " << input_cloud->size () << " data points from room_scan2.pcd" << std::endl;
// Filtering input scan to roughly 10% of original size to increase speed of registration.
pcl::PointCloud<pcl::PointXYZ>::Ptr filtered_cloud (new pcl::PointCloud<pcl::PointXYZ>);
pcl::ApproximateVoxelGrid<pcl::PointXYZ> approximate_voxel_filter;
approximate_voxel_filter.setLeafSize (0.2, 0.2, 0.2);
approximate_voxel_filter.setInputCloud (input_cloud);
approximate_voxel_filter.filter (*filtered_cloud);
std::cout << "Filtered cloud contains " << filtered_cloud->size ()
<< " data points from room_scan2.pcd" << std::endl;
// Initializing Normal Distributions Transform (NDT).
pcl::NormalDistributionsTransform<pcl::PointXYZ, pcl::PointXYZ> ndt;
// Setting scale dependent NDT parameters
// Setting minimum transformation difference for termination condition.
ndt.setTransformationEpsilon (0.01);
// Setting maximum step size for More-Thuente line search.
ndt.setStepSize (0.1);
//Setting Resolution of NDT grid structure (VoxelGridCovariance).
ndt.setResolution (1.0);
// Setting max number of registration iterations.
ndt.setMaximumIterations (35);
// Setting point cloud to be aligned.
ndt.setInputCloud (filtered_cloud);
// Setting point cloud to be aligned to.
ndt.setInputTarget (target_cloud);
// Set initial alignment estimate found using robot odometry.
Eigen::AngleAxisf init_rotation (0.6931, Eigen::Vector3f::UnitZ ());
Eigen::Translation3f init_translation (1.79387, 0.720047, 0);
Eigen::Matrix4f init_guess = (init_translation * init_rotation).matrix ();
// Calculating required rigid transform to align the input cloud to the target cloud.
pcl::PointCloud<pcl::PointXYZ>::Ptr output_cloud (new pcl::PointCloud<pcl::PointXYZ>);
ndt.align (*output_cloud, init_guess);
std::cout << "Normal Distributions Transform has converged:" << ndt.hasConverged ()
<< " score: " << ndt.getFitnessScore () << std::endl;
// Transforming unfiltered, input cloud using found transform.
pcl::transformPointCloud (*input_cloud, *output_cloud, ndt.getFinalTransformation ());
// Saving transformed input cloud.
pcl::io::savePCDFileASCII ("room_scan2_transformed.pcd", *output_cloud);
// Initializing point cloud visualizer
boost::shared_ptr<pcl::visualization::PCLVisualizer>
viewer_final (new pcl::visualization::PCLVisualizer ("3D Viewer"));
viewer_final->setBackgroundColor (0, 0, 0);
// Coloring and visualizing target cloud (red).
pcl::visualization::PointCloudColorHandlerCustom<pcl::PointXYZ>
target_color (target_cloud, 255, 0, 0);
viewer_final->addPointCloud<pcl::PointXYZ> (target_cloud, target_color, "target cloud");
viewer_final->setPointCloudRenderingProperties (pcl::visualization::PCL_VISUALIZER_POINT_SIZE,
1, "target cloud");
// Coloring and visualizing transformed input cloud (green).
pcl::visualization::PointCloudColorHandlerCustom<pcl::PointXYZ>
output_color (output_cloud, 0, 255, 0);
viewer_final->addPointCloud<pcl::PointXYZ> (output_cloud, output_color, "output cloud");
viewer_final->setPointCloudRenderingProperties (pcl::visualization::PCL_VISUALIZER_POINT_SIZE,
1, "output cloud");
// Starting visualizer
viewer_final->addCoordinateSystem (1.0);
viewer_final->initCameraParameters ();
// Wait until visualizer window is closed.
while (!viewer_final->wasStopped ())
{
viewer_final->spinOnce (100);
boost::this_thread::sleep (boost::posix_time::microseconds (100000));
}
return (0);
}
