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;
}
GPXRoute(kGUIString *name) {m_draw=0;m_color=DrawColor(255,0,0);m_name.SetString(name);m_numentries=0;m_entries.SetGrow(true);m_entries.Alloc(16);}
void GameBase::Draw(unsigned int w,unsigned int h)
{
unsigned int i;
unsigned int j;
Ship *ship;
Shot *shot;
Rock *rock;
double scorex,scorey;
char tempstring[64];;
GameParticle *gp;
m_w=w;
m_h=h;
SetScreenCenter(w/2.0f,h/2.0f);
/* draw ships */
if(m_title==false)
{
scorex=(-m_cx)+3.0f;
scorey=(-m_cy)+3.0f;
DrawString("SCORE",1.0f,scorex,scorey,0xffffffff);
scorex+=StringPix("Score",1.0f)+32.0f;
for(i=0;i<m_numplayers;++i)
{
ship=&m_ship[i];
/* show lives */
for(j=0;j<ship->m_life;++j)
{
DrawModel(scorex,scorey+12.0f,0.0f,ship->m_color,ship->m_model,0.40f,0.0f);
scorex+=14.0f;
}
/* draw the score */
sprintf(tempstring,"%08d",ship->m_score);
DrawString(tempstring,1.0f,scorex,scorey,ship->m_color);
scorex+=StringPix(tempstring,1.0f);
if(ship->m_active)
{
if(ship->m_state==STATE_DIE)
{
/* show 4 sides breaking apart */
double h=(ship->m_heading-0.25f)*(PI*2);
for(j=0;j<4;++j)
{
double x=ship->m_x-sin(h)*ship->m_statedelay;
double y=ship->m_y-cos(h)*ship->m_statedelay;;
DrawModel(x,y,ship->m_heading,ship->m_color,&g_exshipmodels[j],1.0f,0.0f);
h+=PI/2.0f;
}
}
else if(ship->m_draw)
{
DrawModel(ship->m_x,ship->m_y,ship->m_heading,ship->m_color,ship->m_model,1.0f,0.0f);
if(ship->m_showthrust)
DrawModel(ship->m_x,ship->m_y,ship->m_heading,ship->m_color,&g_thrustmodel[ship->m_showthrust-1],1.0f,0.0f);
}
}
for(j=0;j<MAXSHOTS;++j)
{
shot=&ship->m_shot[j];
if(shot->m_active)
DrawModel(shot->m_x,shot->m_y,shot->m_heading,ship->m_color,shot->m_model,1.0f,0.0f);
}
}
/* draw ufo */
if(m_ufo.m_active)
DrawModel(m_ufo.m_x,m_ufo.m_y,0.0f,DrawColor(255,255,255),m_ufo.m_model,1.0f,0.0f);
for(j=0;j<MAXSHOTS;++j)
{
shot=&m_ufo.m_shot[j];
if(shot->m_active)
DrawModel(shot->m_x,shot->m_y,shot->m_heading,DrawColor(255,255,255),shot->m_model,1.0f,0.0f);
}
/* draw particles */
gp=m_usedphead.m_next;
while(gp!=&m_usedptail)
{
DrawRect(gp->m_x,gp->m_y,gp->m_x+1,gp->m_y+1,gp->m_color);
gp=gp->m_next;
}
}
/* draw rocks */
for(i=0;i<MAXROCKS;++i)
{
rock=&m_rock[i];
if(rock->m_active)
DrawModel(rock->m_x,rock->m_y,rock->m_heading,DrawColor(255,255,255),rock->m_model,1.0f,0.5f);
}
}
GameModel g_rockmodels[NUMROCKSIZES*NUMROCKMODELS]={
{sizeof(g_rockpoints11)/sizeof(g_rockpoints11[0]),g_rockpoints11},
{sizeof(g_rockpoints12)/sizeof(g_rockpoints12[0]),g_rockpoints12},
{sizeof(g_rockpoints13)/sizeof(g_rockpoints13[0]),g_rockpoints13},
{sizeof(g_rockpoints14)/sizeof(g_rockpoints14[0]),g_rockpoints14},
{sizeof(g_rockpoints21)/sizeof(g_rockpoints21[0]),g_rockpoints21},
{sizeof(g_rockpoints22)/sizeof(g_rockpoints22[0]),g_rockpoints22},
{sizeof(g_rockpoints23)/sizeof(g_rockpoints23[0]),g_rockpoints23},
{sizeof(g_rockpoints24)/sizeof(g_rockpoints24[0]),g_rockpoints24},
{sizeof(g_rockpoints31)/sizeof(g_rockpoints31[0]),g_rockpoints31},
{sizeof(g_rockpoints32)/sizeof(g_rockpoints32[0]),g_rockpoints32},
{sizeof(g_rockpoints33)/sizeof(g_rockpoints33[0]),g_rockpoints33},
{sizeof(g_rockpoints34)/sizeof(g_rockpoints34[0]),g_rockpoints34}};
GameColor GameBase::m_shipcolors[MAXPLAYERS]={
DrawColor(255,0,0),
DrawColor(0,255,0),
DrawColor(0,0,255),
#if MAXPLAYERS==8
DrawColor(255,255,0),
DrawColor(255,0,255),
DrawColor(0,255,255),
DrawColor(128,255,255),
#endif
DrawColor(255,255,255)};
GameVector3 g_decpoints[]={
{6.0f,16.0f,0.0f},
{10.0f,16.0f,1.0f}};
GameModel g_decmodel={sizeof(g_decpoints)/sizeof(g_decpoints[0]),g_decpoints};
static bool HHVM_METHOD(ImagickDraw, color,
double x, double y, int64_t paintMethod) {
auto wand = getDrawingWandResource(this_);
DrawColor(wand->getWand(), x, y, (PaintMethod)paintMethod);
return true;
}
MAGICK_NET_EXPORT void DrawingWand_Color(DrawingWand *instance, const double x, const double y, const size_t paintMethod, ExceptionInfo **exception)
{
DrawColor(instance, x, y, (const PaintMethod)paintMethod);
MAGICK_NET_SET_DRAW_EXCEPTION;
}