47 int main(
int argc,
char **argv)
51 "usage: example4-velodyne <logfile> <start_time> <end time>\n"
53 "start_time and end_time are given in seconds from the\n"
54 "start of the log file\n");
60 fprintf(stderr,
"error opening log file\n");
64 double start_time = strtod(argv[2], NULL);
65 double end_time = strtod(argv[3], NULL);
69 fprintf(stderr,
"couldn't find config file\n");
78 int64_t first_timestamp =
event->timestamp;
81 int64_t start_utime = first_timestamp + (int64_t)(start_time * 1000000);
82 int64_t end_utime = first_timestamp + (int64_t)(end_time * 1000000);
85 memset(&last_pose, 0,
sizeof(last_pose));
98 if(!strcmp(event->channel,
"POSE")) {
106 if(event->timestamp < start_utime) {
111 if(event->timestamp >= end_utime) {
115 if(!strcmp(event->channel,
"VELODYNE")) {
126 double velodyne_to_local[16];
128 velodyne_to_local, &last_pose);
138 if (vsample.
range < 0.01) {
142 double sensor_xyz[4] = {
143 vsample.
xyz[0], vsample.
xyz[1], vsample.
xyz[2], 1
147 sensor_xyz, local_xyz);
149 printf(
"%f %f %f\n", local_xyz[0], local_xyz[1], local_xyz[2]);
convenience functions for small linear algebra operations
int lcmtypes_velodyne_t_decode(const void *buf, int offset, int maxlen, lcmtypes_velodyne_t *p)
Config * config_parse_default(void)
int lcmtypes_pose_t_decode_cleanup(lcmtypes_pose_t *p)
header file to config.cpp Check the LCM instructions for more info
THIS IS AN AUTOMATICALLY GENERATED FILE. DO NOT MODIFY BY HAND!! Generated by LCM.
void lcm_eventlog_destroy(lcm_eventlog_t *l)
lcm_eventlog_event_t * lcm_eventlog_read_next_event(lcm_eventlog_t *l)
header file for eventlog.cpp Check the LCM instructions for more info
header file with velodyne types
velodyne_calib_t * velodyne_calib_create()
int velodyne_decoder_init(velodyne_calib_t *v, velodyne_decoder_t *vd, const void *_data, int datalen)
int main(int argc, char **argv)
int lcmtypes_pose_t_decode(const void *buf, int offset, int maxlen, lcmtypes_pose_t *p)
static void matrix_vector_multiply_4x4_4d(const double m[16], const double v[4], double result[4])
int velodyne_decoder_next(velodyne_calib_t *v, velodyne_decoder_t *vd, struct velodyne_sample *sample)
THIS IS AN AUTOMATICALLY GENERATED FILE. DO NOT MODIFY BY HAND!! Generated by LCM.
int lcmtypes_velodyne_t_decode_cleanup(lcmtypes_velodyne_t *p)
int config_util_sensor_to_local_with_pose(Config *cfg, const char *name, double m[16], lcmtypes_pose_t *p)
void lcm_eventlog_free_event(lcm_eventlog_event_t *le)
header file for config_util.cpp Check the LCM instructions for more info
lcm_eventlog_t * lcm_eventlog_create(const char *path, const char *mode)