void
draw_polygon_rgb(uint8_t *img, int count, const int* xy,
uint32_t color, int fill,
int w, int h, int rowstride)
{
int i, n;
if (count <= 0) return;
if (fill) {
/* Build edge list */
Edge* e = malloc(count * sizeof(Edge));
for (i = n = 0; i < count-1; i++)
add_edge(&e[n++], xy[i+i], xy[i+i+1], xy[i+i+2], xy[i+i+3]);
if (xy[i+i] != xy[0] || xy[i+i+1] != xy[1])
add_edge(&e[n++], xy[i+i], xy[i+i+1], xy[0], xy[1]);
_fill_polygon_rgb(img, n, e, color, w, h, rowstride);
free(e);
} else {
/* Outline */
for (i = 0; i < count-1; i++)
draw_line_rgb(img, xy[i+i], xy[i+i+1], xy[i+i+2], xy[i+i+3],
color, w, h, rowstride);
// draw->line(im, xy[i+i], xy[i+i+1], xy[i+i+2], xy[i+i+3], ink);
draw_line_rgb(img, xy[i+i], xy[i+i+1], xy[0], xy[1], color,
w, h, rowstride);
}
}
int main(int argc, char **argv)
{
uint32_t w = 600;
uint32_t h = 400;
uint32_t rs = w * 3;
uint8_t *img = (uint8_t*)malloc( h * rs );
memset(img, 0, h*rs);
draw_line_rgb(img, 0, 0, 600, 400, 0xFFFF00, w, h, rs);
draw_hline_rgb(img, 0, 200, 200, 0xFF00FF, w, h, rs);
draw_hline_rgb(img, 200, 300, 0, 0xFF00FF, w, h, rs);
int triangle[] = { 300, 200,
300, 100,
400, 200 };
draw_polygon_rgb(img, 3, triangle, 0x00FF00, 0, w, h, rs);
draw_rect_rgb(img, 300, 300, 390, 390, 0xFFFFFF, 1, w, h, rs);
draw_rect_rgb(img, 350, 250, 450, 330, 0x80FF0000, 1, w, h, rs);
draw_rect_rgb(img, 400, 200, 500, 300, 0xa00000FF, 1, w, h, rs);
draw_ellipse_rgb(img, 500, 100, 575, 150, 0x00FFFF, 1, 0,
w, h, rs);
draw_ellipse_rgb(img, 450, 25, 525, 100, 0x00FFFF, 0, 0,
w, h, rs);
draw_ellipse_rgb(img, -50,-50,50,50, 0xFFFF00, 1, 0, w, h, rs);
FILE *fp = fopen("out.ppm", "wb");
ppm_write( fp, img, w, h, w*3 );
fclose( fp );
free( img );
printf("created out.ppm\n");
return 0;
}
int main(int argc, char **argv)
{
const char *optstring = "hc:v";
int c;
struct option long_opts[] = {
{ "help", no_argument, 0, 'h' },
{ "camera", required_argument, 0, 'c' },
{ "visualize", no_argument, 0, 'v' },
{ 0, 0, 0, 0 }
};
std::string camera_id;
bool visualize = false;
while ((c = getopt_long (argc, argv, optstring, long_opts, 0)) >= 0) {
switch (c) {
case 'c':
camera_id = optarg;
break;
case 'v':
visualize = true;
break;
default:
usage();
return 1;
};
}
if(optind >= argc - 1) {
usage();
}
std::string datadir = argv[optind];
std::string outdir = argv[optind+1];
if(camera_id.empty()) {
err("Missing --camera parameter. Run with -h for options.");
return 1;
}
fovis::CameraIntrinsicsParameters camParams;
camParams.width = 640;
camParams.height = 480;
// RGB camera parameters
if(camera_id == "fr1") {
camParams.fx = 517.306408;
camParams.fy = 516.469215;
camParams.cx = 318.643040;
camParams.cy = 255.313989;
camParams.k1 = 0.262383;
camParams.k2 = -0.953104;
camParams.p1 = -0.005358;
camParams.p2 = 0.002628;
camParams.k3 = 1.163314;
} else if(camera_id == "fr2") {
camParams.fx = 520.908620;
camParams.fy = 521.007327;
camParams.cx = 325.141442;
camParams.cy = 249.701764;
camParams.k1 = 0.231222;
camParams.k2 = -0.784899;
camParams.p1 = -0.003257;
camParams.p2 = -0.000105;
camParams.k3 = 0.917205;
} else if(camera_id == "fr3") {
camParams.fx = 537.960322;
camParams.fy = 539.597659;
camParams.cx = 319.183641;
camParams.cy = 247.053820;
camParams.k1 = 0.026370;
camParams.k2 = -0.100086;
camParams.p1 = 0.003138;
camParams.p2 = 0.002421;
camParams.k3 = 0.000000;
} else {
err("Unknown camera id [%s]", camera_id.c_str());
return 1;
}
info("Loading data from: [%s]\n", datadir.c_str());
fovis::Rectification rect(camParams);
// If we wanted to play around with the different VO parameters, we could set
// them here in the "options" variable.
fovis::VisualOdometryOptions options =
fovis::VisualOdometry::getDefaultOptions();
// setup the visual odometry
fovis::VisualOdometry odom(&rect, options);
// create the output trajectory file
std::string traj_fname = outdir + "/traj.txt";
FILE* traj_fp = fopen(traj_fname.c_str(), "w");
if(!traj_fp) {
err("Unable to create %s - %s", traj_fname.c_str(), strerror(errno));
return 1;
}
std::string gray_pgm_dir = outdir + "/gray";
mkdir(gray_pgm_dir.c_str(), 0755);
std::string depth_pgm_dir = outdir + "/depth";
mkdir(depth_pgm_dir.c_str(), 0755);
std::string vis_dir = outdir + "/vis";
mkdir(vis_dir.c_str(), 0755);
// read the RGB and depth index files
std::vector<TimestampFilename> rgb_fnames =
read_file_index(datadir + "/rgb.txt");
std::vector<TimestampFilename> depth_fnames =
read_file_index(datadir + "/depth.txt");
int depth_fname_index = 0;
DrawImage draw_img(camParams.width, camParams.height * 2);
for(int rgb_fname_index=0;
rgb_fname_index < rgb_fnames.size();
rgb_fname_index++) {
int64_t timestamp = rgb_fnames[rgb_fname_index].timestamp;
std::string rgb_fname =
datadir + "/" + rgb_fnames[rgb_fname_index].filename;
long long ts_sec = timestamp / 1000000;
long ts_usec = timestamp % 1000000;
char ts_str[80];
snprintf(ts_str, 80, "%lld.%06ld", ts_sec, ts_usec);
// match the RGB image up with a depth image
bool matched_depth_image = false;
while(depth_fname_index < depth_fnames.size()) {
int64_t depth_ts = depth_fnames[depth_fname_index].timestamp;
// declare a depth image match if the depth image timestamp is within
// 40ms of the RGB image.
int64_t dt_usec = depth_ts - timestamp;
if(dt_usec > 40000) {
dbg(" stop %lld.%06ld (dt %f)",
(long long)(depth_ts / 1000000),
(long)(depth_ts % 1000000),
dt_usec / 1000.);
break;
} else if(dt_usec < -40000) {
dbg(" skip %lld.%06ld (dt %f)",
(long long)(depth_ts / 1000000),
(long)(depth_ts % 1000000),
dt_usec / 1000.);
depth_fname_index++;
} else {
matched_depth_image = true;
dbg(" mtch %lld.%06ld (dt %f)",
(long long)(depth_ts / 1000000),
(long)(depth_ts % 1000000),
dt_usec / 1000.);
break;
}
}
if(!matched_depth_image) {
// didn't find a depth image with a close enough timestamp. Skip this
// RGB image.
info("# skip %s", ts_str);
continue;
}
std::string depth_fname =
datadir + "/" + depth_fnames[depth_fname_index].filename;
depth_fname_index++;
// read RGB data
std::vector<uint8_t> rgb_data;
unsigned rgb_width;
unsigned rgb_height;
std::vector<uint8_t> rgb_png_data;
lodepng::load_file(rgb_png_data, rgb_fname.c_str());
if(rgb_png_data.empty()) {
err("Failed to load %s", rgb_fname.c_str());
continue;
}
if(0 != lodepng::decode(rgb_data, rgb_width, rgb_height,
rgb_png_data, LCT_RGB, 8)) {
err("Error decoding PNG %s", rgb_fname.c_str());
continue;
}
// convert RGB data to grayscale
std::vector<uint8_t> gray_data(rgb_width*rgb_height);
const uint8_t* rgb_pixel = &rgb_data[0];
for(int pixel_index=0; pixel_index<rgb_width*rgb_height; pixel_index++) {
uint8_t r = rgb_pixel[0];
uint8_t g = rgb_pixel[1];
uint8_t b = rgb_pixel[2];
gray_data[pixel_index] = (r + g + b) / 3;
rgb_pixel += 3;
}
// write gray image out.
write_pgm(outdir + "/gray/" + ts_str + "-gray.pgm",
&gray_data[0], rgb_width, rgb_height, rgb_width);
// read depth data
std::vector<uint8_t> depth_data_u8;
unsigned depth_width;
unsigned depth_height;
std::vector<uint8_t> depth_png_data;
lodepng::load_file(depth_png_data, depth_fname.c_str());
if(depth_png_data.empty()) {
err("Failed to load %s", depth_fname.c_str());
continue;
}
if(0 != lodepng::decode(depth_data_u8, depth_width, depth_height,
depth_png_data, LCT_GREY, 16)) {
err("Error decoding PNG %s", depth_fname.c_str());
continue;
}
// convert depth data to a DepthImage object
fovis::DepthImage depth_image(camParams, depth_width, depth_height);
std::vector<float> depth_data(depth_width * depth_height);
for(int i=0; i<depth_width*depth_height; i++) {
// lodepng loads 16-bit PNGs into memory with big-endian ordering.
// swizzle the bytes to get a uint16_t
uint8_t high_byte = depth_data_u8[i*2];
uint8_t low_byte = depth_data_u8[i*2+1];
uint16_t data16 = (high_byte << 8) | low_byte;
if(0 == data16)
depth_data[i] = NAN;
else
depth_data[i] = data16 / float(5000);
}
depth_image.setDepthImage(&depth_data[0]);
// debug depth image
std::vector<uint8_t> depth_pgm_data(depth_width * depth_height);
float depth_pgm_max_depth = 5;
for(int i=0; i<depth_width*depth_height; i++) {
if(isnan(depth_data[i]))
depth_pgm_data[i] = 0;
else {
float d = depth_data[i];
if(d > depth_pgm_max_depth)
d = depth_pgm_max_depth;
depth_pgm_data[i] = 255 * d / depth_pgm_max_depth;
}
}
write_pgm(outdir + "/depth/" + ts_str + "-depth.pgm",
&depth_pgm_data[0], depth_width, depth_height, depth_width);
// process the frame
odom.processFrame(&gray_data[0], &depth_image);
fovis::MotionEstimateStatusCode status = odom.getMotionEstimateStatus();
switch(status) {
case fovis::INSUFFICIENT_INLIERS:
printf("# %s - insufficient inliers\n", ts_str);
break;
case fovis::OPTIMIZATION_FAILURE:
printf("# %s - optimization failed\n", ts_str);
break;
case fovis::REPROJECTION_ERROR:
printf("# %s - reprojection error too high\n", ts_str);
break;
case fovis::NO_DATA:
printf("# %s - no data\n", ts_str);
break;
case fovis::SUCCESS:
default:
break;
}
// get the integrated pose estimate.
Eigen::Isometry3d cam_to_local = odom.getPose();
Eigen::Vector3d t = cam_to_local.translation();
Eigen::Quaterniond q(cam_to_local.rotation());
printf("%lld.%06ld %f %f %f %f %f %f %f\n",
ts_sec, ts_usec, t.x(), t.y(), t.z(), q.x(), q.y(), q.z(), q.w());
fprintf(traj_fp, "%lld.%06ld %f %f %f %f %f %f %f\n",
ts_sec, ts_usec, t.x(), t.y(), t.z(), q.x(), q.y(), q.z(), q.w());
// visualization
if(visualize) {
DrawColor status_colors[] = {
DrawColor(255, 255, 0),
DrawColor(0, 255, 0),
DrawColor(255, 0, 0),
DrawColor(255, 0, 255),
DrawColor(127, 127, 0)
};
DrawColor red(255, 0, 0);
DrawColor green(0, 255, 0);
DrawColor blue(0, 0, 255);
memset(&draw_img.data[0], 0, draw_img.data.size());
const fovis::OdometryFrame* ref_frame = odom.getReferenceFrame();
const fovis::OdometryFrame* tgt_frame = odom.getTargetFrame();
const fovis::PyramidLevel* ref_pyr_base = ref_frame->getLevel(0);
const uint8_t* ref_img_data = ref_pyr_base->getGrayscaleImage();
int ref_img_stride = ref_pyr_base->getGrayscaleImageStride();
const fovis::PyramidLevel* tgt_pyr_base = tgt_frame->getLevel(0);
const uint8_t* tgt_img_data = tgt_pyr_base->getGrayscaleImage();
int tgt_img_stride = tgt_pyr_base->getGrayscaleImageStride();
int tgt_yoff = ref_pyr_base->getHeight();
// draw the reference frame on top
draw_gray_img_rgb(ref_img_data, ref_pyr_base->getWidth(),
ref_pyr_base->getHeight(), ref_img_stride, 0, 0, &draw_img);
// draw the target frame on bottom
draw_gray_img_rgb(tgt_img_data, tgt_pyr_base->getWidth(),
tgt_pyr_base->getHeight(), tgt_img_stride, 0, tgt_yoff, &draw_img);
const fovis::MotionEstimator* mestimator = odom.getMotionEstimator();
int num_levels = ref_frame->getNumLevels();
for(int level_index=0; level_index<num_levels; level_index++) {
// draw reference features
const fovis::PyramidLevel* ref_level = ref_frame->getLevel(level_index);
int num_ref_keypoints = ref_level->getNumKeypoints();
for(int ref_kp_ind=0; ref_kp_ind<num_ref_keypoints; ref_kp_ind++) {
const fovis::KeypointData* ref_kp = ref_level->getKeypointData(ref_kp_ind);
int ref_u = (int)round(ref_kp->base_uv.x());
int ref_v = (int)round(ref_kp->base_uv.y());
draw_box_rgb(ref_u-1, ref_v-1, ref_u+1, ref_v+1, blue, &draw_img);
}
// draw target features
const fovis::PyramidLevel* tgt_level = tgt_frame->getLevel(level_index);
int num_tgt_keypoints = tgt_level->getNumKeypoints();
for(int tgt_kp_ind=0; tgt_kp_ind<num_tgt_keypoints; tgt_kp_ind++) {
const fovis::KeypointData* tgt_kp = tgt_level->getKeypointData(tgt_kp_ind);
int tgt_u = (int)round(tgt_kp->base_uv.x());
int tgt_v = (int)round(tgt_kp->base_uv.y());
draw_box_rgb(tgt_u-1, tgt_v-1 + tgt_yoff, tgt_u+1, tgt_v+1 + tgt_yoff,
blue, &draw_img);
}
}
const fovis::FeatureMatch* matches = mestimator->getMatches();
int num_matches = mestimator->getNumMatches();
// draw non-inlier matches
for(int match_index=0; match_index<num_matches; match_index++) {
const fovis::FeatureMatch& match = matches[match_index];
if(match.inlier)
continue;
const fovis::KeypointData* ref_keypoint = match.ref_keypoint;
const fovis::KeypointData* tgt_keypoint = match.target_keypoint;
int ref_u = (int)round(ref_keypoint->base_uv.x());
int ref_v = (int)round(ref_keypoint->base_uv.y());
int tgt_u = (int)round(tgt_keypoint->base_uv.x());
int tgt_v = (int)round(tgt_keypoint->base_uv.y());
draw_line_rgb(ref_u, ref_v,
tgt_u, tgt_v + tgt_yoff,
red, &draw_img);
}
// draw inlier matches
for(int match_index=0; match_index<num_matches; match_index++) {
const fovis::FeatureMatch& match = matches[match_index];
if(!match.inlier)
continue;
const fovis::KeypointData* ref_keypoint = match.ref_keypoint;
const fovis::KeypointData* tgt_keypoint = match.target_keypoint;
int ref_u = (int)round(ref_keypoint->base_uv.x());
int ref_v = (int)round(ref_keypoint->base_uv.y());
int tgt_u = (int)round(tgt_keypoint->base_uv.x());
int tgt_v = (int)round(tgt_keypoint->base_uv.y());
draw_line_rgb(ref_u, ref_v,
tgt_u, tgt_v + tgt_yoff,
green, &draw_img);
}
// draw a couple lines indicating the VO status
draw_box_rgb(0, tgt_yoff - 1, draw_img.width,
tgt_yoff + 1, status_colors[status], &draw_img);
// save visualization
std::string vis_fname = vis_dir + "/" + ts_str + "-vis.png";
lodepng::encode(vis_fname, &draw_img.data[0], draw_img.width,
draw_img.height, LCT_RGB);
}
}
fclose(traj_fp);
return 0;
}
static int
_draw_ellipse_rgb(uint8_t *img, int x0, int y0, int x1, int y1,
int start, int end, uint32_t color, int fill, int mode, int op,
int img_width, int img_height, int rowstride)
{
int i, n;
int cx, cy;
int w, h;
int x = 0, y = 0;
int lx = 0, ly = 0;
int sx = 0, sy = 0;
w = x1 - x0;
h = y1 - y0;
if (w < 0 || h < 0) return 0;
cx = (x0 + x1) / 2;
cy = (y0 + y1) / 2;
while (end < start) end += 360;
if (mode != ARC && fill) {
/* Build edge list */
Edge* e = malloc((end - start + 3) * sizeof(Edge));
if( !e ) abort();
n = 0;
for (i = start; i <= end; i++) {
x = FLOOR((cos(i*M_PI/180) * w/2) + cx + 0.5);
y = FLOOR((sin(i*M_PI/180) * h/2) + cy + 0.5);
if (i != start)
add_edge(&e[n++], lx, ly, x, y);
else
sx = x, sy = y;
lx = x, ly = y;
}
if (n > 0) {
/* close and draw polygon */
if (mode == PIESLICE) {
if (x != cx || y != cy) {
add_edge(&e[n++], x, y, cx, cy);
add_edge(&e[n++], cx, cy, sx, sy);
}
} else {
if (x != sx || y != sy)
add_edge(&e[n++], x, y, sx, sy);
}
_fill_polygon_rgb(img, n, e, color,
img_width, img_height, rowstride);
}
free(e);
} else {
for (i = start; i <= end; i++) {
x = FLOOR((cos(i*M_PI/180) * w/2) + cx + 0.5);
y = FLOOR((sin(i*M_PI/180) * h/2) + cy + 0.5);
if (i != start)
draw_line_rgb(img, lx, ly, x, y, color,
img_width, img_height, rowstride);
else
sx = x, sy = y;
lx = x, ly = y;
}
if (i != start) {
if (mode == PIESLICE) {
if (x != cx || y != cy) {
draw_line_rgb(img, x, y, cx, cy, color,
img_width, img_height, rowstride);
draw_line_rgb(img, cx, cy, sx, sy, color,
img_width, img_height, rowstride);
}
} else if (mode == CHORD) {
if (x != sx || y != sy)
draw_line_rgb(img, x, y, sx, sy, color,
img_width, img_height, rowstride);
}
}
}
return 0;
}
