void Trajectory::PlotTransformedTrajectory(bot_lcmgl_t *lcmgl, const BotTrans *transform)
{
bot_lcmgl_line_width(lcmgl, 2.0f);
bot_lcmgl_begin(lcmgl, GL_LINE_STRIP);
for (int i=0; i<int(xpoints_.size()); i++)
{
double xyz[3];
GetTransformedPoint(i, transform, xyz);
bot_lcmgl_vertex3f(lcmgl, xyz[0], xyz[1], xyz[2]);
}
bot_lcmgl_end(lcmgl);
}
int main(int argc, char** argv)
{
lcm_t* lcm_ = lcm_create(NULL);
bot_lcmgl_t* lcmgl_ = bot_lcmgl_init(lcm_, "LCMGL_DEMO");
// lcmgl setup
bot_lcmgl_translated(lcmgl_, 0, 0, 0);
bot_lcmgl_line_width(lcmgl_, 2.0f);
bot_lcmgl_point_size(lcmgl_, 12.0f);
bot_lcmgl_begin(lcmgl_, GL_POINTS);
// setup color and draw a point
bot_lcmgl_color3f(lcmgl_, 1.0, 0, 0);
bot_lcmgl_vertex3f(lcmgl_, 0, 0, 1);
// end and refresh to viewer
bot_lcmgl_end(lcmgl_);
bot_lcmgl_switch_buffer(lcmgl_);
printf("Dosvedanya!\n");
return 0;
}
void
Visualization::draw_pyramid_level_matches(const VisualOdometry* odom, int level_num)
{
const OdometryFrame* ref_frame = odom->getReferenceFrame();
const OdometryFrame* target_frame = odom->getTargetFrame();
const PyramidLevel* ref_level = ref_frame->getLevel(level_num);
const PyramidLevel* target_level = target_frame->getLevel(level_num);
int width = ref_level->getWidth();
int height = ref_level->getHeight();
const MotionEstimator* estimator = odom->getMotionEstimator();
const FeatureMatch* matches = estimator->getMatches();
int num_matches = estimator->getNumMatches();
// previous image
bot_lcmgl_color3f(_lcmgl, 1,1,1);
const uint8_t* ref_gray = ref_level->getGrayscaleImage();
int ref_gray_stride = ref_level->getGrayscaleImageStride();
int prev_gray_texid = bot_lcmgl_texture2d(_lcmgl, ref_gray,
width, height, ref_gray_stride,
BOT_LCMGL_LUMINANCE, BOT_LCMGL_UNSIGNED_BYTE, BOT_LCMGL_COMPRESS_NONE);
bot_lcmgl_push_matrix(_lcmgl);
bot_lcmgl_translated(_lcmgl, 0, height + 10, 0);
bot_lcmgl_texture_draw_quad(_lcmgl, prev_gray_texid,
0, 0, 0,
0, height, 0,
width, height, 0,
width, 0, 0);
// draw features in reference frame
bot_lcmgl_color3f(_lcmgl, 1, 0, 1);
bot_lcmgl_point_size(_lcmgl, 1.5f);
bot_lcmgl_begin(_lcmgl, GL_POINTS);
for(int i=0, nfeatures=ref_level->getNumKeypoints(); i<nfeatures; i++) {
const KeyPoint& kp = ref_level->getKeypoint(i);
bot_lcmgl_vertex2f(_lcmgl, kp.u, kp.v);
}
bot_lcmgl_end(_lcmgl);
bot_lcmgl_pop_matrix(_lcmgl);
// current image
bot_lcmgl_color3f(_lcmgl, 1,1,1);
const uint8_t* target_gray = target_level->getGrayscaleImage();
int target_gray_stride = target_level->getGrayscaleImageStride();
int gray_texid = bot_lcmgl_texture2d(_lcmgl, target_gray,
width, height, target_gray_stride,
BOT_LCMGL_LUMINANCE, BOT_LCMGL_UNSIGNED_BYTE, BOT_LCMGL_COMPRESS_NONE);
bot_lcmgl_texture_draw_quad(_lcmgl, gray_texid,
0, 0, 0,
0, height, 0,
width, height, 0,
width, 0, 0);
// draw features
bot_lcmgl_color3f(_lcmgl, 0, 1, 0);
bot_lcmgl_point_size(_lcmgl, 3.0f);
bot_lcmgl_begin(_lcmgl, GL_POINTS);
for(int i=0, nfeatures=target_level->getNumKeypoints(); i<nfeatures; i++) {
const KeyPoint& kp = target_level->getKeypoint(i);
bot_lcmgl_vertex2f(_lcmgl, kp.u, kp.v);
}
bot_lcmgl_end(_lcmgl);
// draw matches that are not in the maximal clique
bot_lcmgl_color3f(_lcmgl, 0.3, 0, 0);
bot_lcmgl_begin(_lcmgl, GL_LINES);
for(int i=0; i<num_matches; i++) {
const FeatureMatch& match = matches[i];
if(match.inlier || match.in_maximal_clique || match.target_keypoint->pyramid_level != level_num)
continue;
int cur_x = match.target_keypoint->kp.u;
int cur_y = match.target_keypoint->kp.v;
int prev_x = match.ref_keypoint->kp.u;
int prev_y = match.ref_keypoint->kp.v;
bot_lcmgl_vertex2f(_lcmgl, cur_x, cur_y);
bot_lcmgl_vertex2f(_lcmgl, prev_x, prev_y + height + 10);
}
bot_lcmgl_end(_lcmgl);
// draw inliers
bot_lcmgl_color3f(_lcmgl, 0, 0, 1);
bot_lcmgl_line_width(_lcmgl, 2.0);
bot_lcmgl_begin(_lcmgl, GL_LINES);
for(int i=0; i<num_matches; i++) {
const FeatureMatch& match = matches[i];
if(!match.inlier || match.target_keypoint->pyramid_level != level_num)
continue;
int cur_x = match.target_keypoint->kp.u;
int cur_y = match.target_keypoint->kp.v;
int prev_x = match.ref_keypoint->kp.u;
int prev_y = match.ref_keypoint->kp.v;
bot_lcmgl_vertex2f(_lcmgl, cur_x, cur_y);
bot_lcmgl_vertex2f(_lcmgl, prev_x, prev_y + height + 10);
}
bot_lcmgl_end(_lcmgl);
// draw matches that are in the maximal clique but failed the projection test
bot_lcmgl_line_width(_lcmgl, 1.0);
for(int i=0; i<num_matches; i++) {
const FeatureMatch& match = matches[i];
if(match.in_maximal_clique && !match.inlier && match.target_keypoint->pyramid_level == level_num) {
int cur_x = match.target_keypoint->kp.u;
int cur_y = match.target_keypoint->kp.v;
int prev_x = match.ref_keypoint->kp.u;
int prev_y = match.ref_keypoint->kp.v;
bot_lcmgl_color3f(_lcmgl, 1, 0, 0);
bot_lcmgl_begin(_lcmgl, GL_LINES);
bot_lcmgl_vertex2f(_lcmgl, cur_x, cur_y);
bot_lcmgl_vertex2f(_lcmgl, prev_x, prev_y + height + 10);
bot_lcmgl_end(_lcmgl);
bot_lcmgl_color3f(_lcmgl, 1, 1, 1);
double cur_xyz[] = { cur_x, cur_y + 10, 0 };
char txt[500];
snprintf(txt, 80, "%.3f", match.reprojection_error);
bot_lcmgl_text(_lcmgl, cur_xyz, txt);
}
}
if (level_num ==0){
//draw the ESM homography estimate
bot_lcmgl_line_width(_lcmgl, 2.0);
bot_lcmgl_color3f(_lcmgl, 1, 1, 0);
bot_lcmgl_begin(_lcmgl,GL_LINE_STRIP);
const Eigen::Matrix3d & H = odom->getInitialHomography();
Eigen::MatrixXd vertices(5, 3);
vertices <<
0 , 0 , 1 ,
width , 0 , 1 ,
width , height , 1 ,
0 , height , 1 ,
0 , 0 , 1;
Eigen::MatrixXd warpedPoints = H*vertices.transpose();
warpedPoints.row(0) = warpedPoints.row(0).array()/warpedPoints.row(2).array();
warpedPoints.row(1) = warpedPoints.row(1).array()/warpedPoints.row(2).array();
for (int i=0;i<warpedPoints.cols();i++){
bot_lcmgl_vertex2f(_lcmgl, warpedPoints(0, i), warpedPoints(1, i));
}
bot_lcmgl_end(_lcmgl);
}
}
void
Visualization::draw_pyramid_level_flow(const VisualOdometry* odom, int level_num)
{
const OdometryFrame* ref_frame = odom->getReferenceFrame();
const OdometryFrame* target_frame = odom->getTargetFrame();
const PyramidLevel* ref_level = ref_frame->getLevel(level_num);
const PyramidLevel* target_level = target_frame->getLevel(level_num);
int width = ref_level->getWidth();
int height = ref_level->getHeight();
const MotionEstimator* estimator = odom->getMotionEstimator();
const FeatureMatch* matches = estimator->getMatches();
int num_matches = estimator->getNumMatches();
// current image
bot_lcmgl_color3f(_lcmgl, 1, 1, 1);
const uint8_t* target_gray = target_level->getGrayscaleImage();
int target_gray_stride = target_level->getGrayscaleImageStride();
int gray_texid = bot_lcmgl_texture2d(_lcmgl, target_gray, width, height,
target_gray_stride, BOT_LCMGL_LUMINANCE,
BOT_LCMGL_UNSIGNED_BYTE,
BOT_LCMGL_COMPRESS_NONE);
bot_lcmgl_texture_draw_quad(_lcmgl, gray_texid,
0 , 0 , 0 ,
0 , height , 0 ,
width , height , 0 ,
width , 0 , 0);
float rgb[3];
#if 0
// draw target features
bot_lcmgl_color3f(_lcmgl, 0, 1, 0);
bot_lcmgl_point_size(_lcmgl, 3.0f);
bot_lcmgl_begin(_lcmgl, GL_POINTS);
for(int i=0, nfeatures=target_level->getNumKeypoints(); i<nfeatures; ++i) {
const KeypointData& kpdata(*target_level->getKeypointData(i));
colormap(kpdata.xyz.z(), rgb);
bot_lcmgl_color3f(_lcmgl, rgb[0], rgb[1], rgb[2]);
bot_lcmgl_vertex2f(_lcmgl, kpdata.kp.u, kpdata.kp.v);
}
bot_lcmgl_end(_lcmgl);
#endif
#if 0
// draw 9x9 boxes around keypoints
bot_lcmgl_line_width(_lcmgl, 1.0);
bot_lcmgl_color3f(_lcmgl, .5, .5, 1);
bot_lcmgl_begin(_lcmgl, GL_LINES);
for(int i=0, num_kp=target_level->getNumKeypoints();
i < num_kp;
++i) {
const KeypointData& kpdata(*target_level->getKeypointData(i));
colormap(kpdata.xyz.z(), rgb);
bot_lcmgl_color3f(_lcmgl, rgb[0], rgb[1], rgb[2]);
bot_lcmgl_vertex2f(_lcmgl, kpdata.kp.u-4, kp.v-4);
bot_lcmgl_vertex2f(_lcmgl, kpdata.kp.u-4, kp.v+4);
bot_lcmgl_vertex2f(_lcmgl, kpdata.kp.u-4, kp.v+4);
bot_lcmgl_vertex2f(_lcmgl, kpdata.kp.u+4, kp.v+4);
bot_lcmgl_vertex2f(_lcmgl, kpdata.kp.u+4, kp.v+4);
bot_lcmgl_vertex2f(_lcmgl, kpdata.kp.u+4, kp.v-4);
bot_lcmgl_vertex2f(_lcmgl, kpdata.kp.u+4, kp.v-4);
bot_lcmgl_vertex2f(_lcmgl, kpdata.kp.u-4, kp.v-4);
}
bot_lcmgl_end(_lcmgl);
#endif
#if 1
// draw inliers
bot_lcmgl_point_size(_lcmgl, 4.0f);
bot_lcmgl_begin(_lcmgl, GL_POINTS);
for (int i=0; i<num_matches; i++) {
const FeatureMatch& match = matches[i];
if (!match.inlier ||
match.target_keypoint->pyramid_level != level_num)
continue;
int cur_x = match.target_keypoint->kp.u;
int cur_y = match.target_keypoint->kp.v;
colormap(match.target_keypoint->xyz(2), rgb);
bot_lcmgl_color3f(_lcmgl, rgb[0], rgb[1], rgb[2]);
bot_lcmgl_vertex2f(_lcmgl, cur_x, cur_y);
}
bot_lcmgl_end(_lcmgl);
#endif
#if 1
// draw ref-to-target 'flow'
//bot_lcmgl_color3f(_lcmgl, 0, 1, 0);
bot_lcmgl_line_width(_lcmgl, 2.0f);
bot_lcmgl_begin(_lcmgl, GL_LINES);
for (int i=0; i<num_matches; i++) {
const FeatureMatch& match = matches[i];
if (!match.inlier ||
match.target_keypoint->pyramid_level != level_num)
continue;
int cur_x = match.target_keypoint->kp.u;
int cur_y = match.target_keypoint->kp.v;
int prev_x = match.ref_keypoint->kp.u;
int prev_y = match.ref_keypoint->kp.v;
colormap(match.target_keypoint->xyz(2), rgb);
bot_lcmgl_color3f(_lcmgl, rgb[0], rgb[1], rgb[2]);
bot_lcmgl_vertex2f(_lcmgl, cur_x, cur_y);
bot_lcmgl_vertex2f(_lcmgl, prev_x, prev_y);
}
bot_lcmgl_end(_lcmgl);
#endif
if (level_num == 0) {
//draw the ESM homography estimate
bot_lcmgl_line_width(_lcmgl, 2.0);
bot_lcmgl_color3f(_lcmgl, 1, 1, 0);
bot_lcmgl_begin(_lcmgl, GL_LINE_STRIP);
const Eigen::Matrix3d & H = odom->getInitialHomography();
Eigen::MatrixXd vertices(5, 3);
vertices <<
0 , 0 , 1,
width , 0 , 1,
width , height , 1,
0 , height , 1,
0 , 0 , 1;
Eigen::MatrixXd warpedPoints = H*vertices.transpose();
warpedPoints.row(0) = warpedPoints.row(0).array()/warpedPoints.row(2).array();
warpedPoints.row(1) = warpedPoints.row(1).array()/warpedPoints.row(2).array();
for (int i=0;i<warpedPoints.cols();i++) {
bot_lcmgl_vertex2f(_lcmgl, warpedPoints(0, i) ,warpedPoints(1, i));
}
bot_lcmgl_end(_lcmgl);
}
}
