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); } }