TEST_F(TrajectoryLibraryTest, ManyTrajectoriesWithTransform) { StereoOctomap octomap(bot_frames_); TrajectoryLibrary lib(0); lib.LoadLibrary("trajtest/full", true); double altitude = 30; double point[3] = {18, 12, 0}; AddPointToOctree(&octomap, point, altitude); BotTrans trans; bot_trans_set_identity(&trans); trans.trans_vec[0] = 17; trans.trans_vec[1] = 11; trans.trans_vec[2] = altitude; // send th double dist; const Trajectory *best_traj; std::tie(dist, best_traj) = lib.FindFarthestTrajectory(octomap, trans, 5.0); DrawOriginLcmGl(lcm_); bot_lcmgl_color3f(lcmgl_, 0, 0, 1); best_traj->Draw(lcmgl_, &trans); EXPECT_EQ_ARM(best_traj->GetTrajectoryNumber(), 3); EXPECT_NEAR(dist, 1.025243, TOLERANCE); // now add a yaw trans.rot_quat[0] = 0.642787609686539; trans.rot_quat[1] = 0; trans.rot_quat[2] = 0; trans.rot_quat[3] = 0.766044443118978; bot_lcmgl_color3f(lcmgl_, 1, 0, 0); best_traj->Draw(lcmgl_, &trans); bot_lcmgl_switch_buffer(lcmgl_); std::tie(dist, best_traj) = lib.FindFarthestTrajectory(octomap, trans, 5.0); //lib.Draw(lcm_, &trans); EXPECT_EQ_ARM(best_traj->GetTrajectoryNumber(), 2); EXPECT_NEAR(dist, 1.174604, TOLERANCE); // now have a transform with roll, pitch, and yaw trans.rot_quat[0] = 0.863589399067779; trans.rot_quat[1] = -0.004581450790098; trans.rot_quat[2] = 0.298930259006064; trans.rot_quat[3] = 0.405996379758463; std::tie(dist, best_traj) = lib.FindFarthestTrajectory(octomap, trans, 5.0); EXPECT_EQ_ARM(best_traj->GetTrajectoryNumber(), 4); EXPECT_NEAR(dist, 0.327772, TOLERANCE); }
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; }
/** * Tests an obstacle appearing during a trajectory execution */ TEST_F(StateMachineControlTest, TrajectoryInterrupt) { StateMachineControl *fsm_control = new StateMachineControl(lcm_, "../TrajectoryLibrary/trajtest/full", "tvlqr-action-out", "state-machine-state", "altitude-reset", false); //fsm_control->GetFsmContext()->setDebugFlag(true); SubscribeLcmChannels(fsm_control); ForceAutonomousMode(); float altitude = 100; // send an obstacle to get it to transition to a new time mav::pose_t msg = GetDefaultPoseMsg(); lcm_->publish(pose_channel_, &msg); ProcessAllLcmMessages(fsm_control); float point[3] = { 24, 0, 0+altitude }; SendStereoPointTriple(point); ProcessAllLcmMessages(fsm_control); lcm_->publish(pose_channel_, &msg); ProcessAllLcmMessages(fsm_control); // ensure that we have changed trajectories EXPECT_EQ_ARM(fsm_control->GetCurrentTrajectory().GetTrajectoryNumber(), 2); // from matlab Trajectory running_traj = fsm_control->GetCurrentTrajectory(); // wait for that trajectory to time out int64_t t_start = GetTimestampNow(); double t = 0; while (t < 1.0) { usleep(7142); // 1/140 of a second msg.utime = GetTimestampNow(); t = (msg.utime - t_start) / 1000000.0; Eigen::VectorXd state_t = running_traj.GetState(t); msg.pos[0] = state_t(0); msg.pos[1] = state_t(1); msg.pos[2] = state_t(2) + altitude; msg.vel[0] = state_t(6); msg.vel[1] = state_t(7); msg.vel[2] = state_t(8); double rpy[3]; rpy[0] = state_t(3); rpy[1] = state_t(4); rpy[2] = state_t(5); double quat[4]; bot_roll_pitch_yaw_to_quat(rpy, quat); msg.orientation[0] = quat[0]; msg.orientation[1] = quat[1]; msg.orientation[2] = quat[2]; msg.orientation[3] = quat[3]; msg.rotation_rate[0] = state_t(9); msg.rotation_rate[1] = state_t(10); msg.rotation_rate[2] = state_t(11); lcm_->publish(pose_channel_, &msg); ProcessAllLcmMessages(fsm_control); } // now add a new obstacle right in front! std::cout << "NEW POINT" << std::endl; lcm_->publish(pose_channel_, &msg); ProcessAllLcmMessages(fsm_control); float point2[3] = { 18, 12, 0+altitude }; SendStereoPointTriple(point2); ProcessAllLcmMessages(fsm_control); lcm_->publish(pose_channel_, &msg); ProcessAllLcmMessages(fsm_control); fsm_control->GetOctomap()->Draw(lcm_->getUnderlyingLCM()); BotTrans body_to_local; bot_frames_get_trans(bot_frames_, "body", "local", &body_to_local); fsm_control->GetTrajectoryLibrary()->Draw(lcm_->getUnderlyingLCM(), &body_to_local); fsm_control->GetTrajectoryLibrary()->Draw(lcm_->getUnderlyingLCM(), &body_to_local); bot_lcmgl_t *lcmgl = bot_lcmgl_init(lcm_->getUnderlyingLCM(), "Trjaectory"); bot_lcmgl_color3f(lcmgl, 0, 1, 0); fsm_control->GetCurrentTrajectory().Draw(lcmgl ,&body_to_local); bot_lcmgl_switch_buffer(lcmgl); bot_lcmgl_destroy(lcmgl); EXPECT_EQ_ARM(fsm_control->GetCurrentTrajectory().GetTrajectoryNumber(), 3); // from matlab delete fsm_control; UnsubscribeLcmChannels(); }
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); } }