int main(int argc, char** argv) {
srand(0);
cl = std::shared_ptr<GetPot>(new GetPot(argc, argv));
if (cl->search("--help")) {
LOG(INFO) << g_usage;
exit(-1);
}
if (cl->search("-startnow")) {
is_running = true;
}
LOG(INFO) << "Initializing camera...";
LoadCameras();
InitTracker();
InitGui();
ba::debug_level_threshold = -1;
Run();
return 0;
}
//
// Init GL
//
int InitGL(GLvoid) // All Setup For OpenGL Goes Here
{
// seed rand generator
srand((unsigned int)time(NULL));
// Load the key codes --
LoadKeyCodes();
// enable all the goodies
Super_LoadGlobals();
Load_ConfigFile();
Super_FireAnts();
Create_Col_List();
Create_Wall_List();
//
// loading sound library
printf("Loading sound library, make sure 'music apps' are closed\n");
Load_Audio();
glShadeModel(GL_SMOOTH); // Enable Smooth Shading
glClearColor(0.0f, 0.0f, 0.0f, 0.0f); // Black Background
glClearDepth(1.0f); // Depth Buffer Setup
glEnable(GL_NORMALIZE);
glPolygonMode(GL_FRONT_AND_BACK, GL_FILL);
glShadeModel(GL_SMOOTH);
glEnable(GL_DEPTH_TEST); // Enables Depth Testing
glDepthFunc(GL_LEQUAL); // The Type Of Depth Testing To Do
glHint(GL_PERSPECTIVE_CORRECTION_HINT, GL_NICEST); // Really Nice Perspective Calculations
// have to load the quadrice to be used
quadric=gluNewQuadric(); // Create A Pointer To The Quadric Object (NEW)
gluQuadricNormals(quadric, GLU_SMOOTH); // Create Smooth Normals (NEW)
gluQuadricTexture(quadric, GL_TRUE); // Create Texture Coords (NEW)
GenerateFireAnts();
CreateWorld();
LoadCameras();
// Load the objects
InitObjects();
GenerateLights();
InitGlobals();
Build_ParticleSet();
Reset_FontID();
// Load the title screen text
Load_Titles();
// load the text library
Super_MainText();
CreateWalls();
//
// Load the collsion test for the walll
//
// front wall
InsertColSegment(world_ptr->x_min, world_ptr->y_max,
world_ptr->x_max, world_ptr->y_max);
// right wall
InsertColSegment(world_ptr->x_max, world_ptr->y_min,
world_ptr->x_max, world_ptr->y_max);
// back wall
InsertColSegment(world_ptr->x_min, world_ptr->y_min,
world_ptr->x_max, world_ptr->y_min);
// left wall
InsertColSegment(world_ptr->x_min, world_ptr->y_min,
world_ptr->x_min, world_ptr->y_max);
//
// for the network save
// the number of bots incase it is changed
MAX_SAVED_BOTS = MAX_FIRE_ANTS;
// end of insertion
Super_InitNetwork();
//
// begin in paused mode
//
Super_BeginPaused();
mSuper_Loaded = LOADED_TRUE;
return true; // Initialization Went OK
} // end of the function
bool CarGraphics::Load(
const PTree & cfg,
const std::string & carpath,
const std::string & /*carname*/,
const std::string & carwheel,
const std::string & carpaint,
const Vec3 & carcolor,
const int anisotropy,
const float camerabounce,
ContentManager & content,
std::ostream & error_output)
{
assert(!loaded);
// init drawable load functor
LoadDrawable loadDrawable(carpath, anisotropy, content, models, textures, error_output);
// load body first
const PTree * cfg_body;
std::string meshname;
std::vector<std::string> texname;
if (!cfg.get("body", cfg_body, error_output)) return false;
if (!cfg_body->get("mesh", meshname, error_output)) return false;
if (!cfg_body->get("texture", texname, error_output)) return false;
if (carpaint != "default") texname[0] = carpaint;
if (!loadDrawable(meshname, texname, *cfg_body, topnode, &bodynode)) return false;
// load wheels
const PTree * cfg_wheels;
if (!cfg.get("wheel", cfg_wheels, error_output)) return false;
std::shared_ptr<PTree> sel_wheel;
if (carwheel != "default" && !content.load(sel_wheel, carpath, carwheel)) return false;
for (PTree::const_iterator i = cfg_wheels->begin(); i != cfg_wheels->end(); ++i)
{
const PTree * cfg_wheel = &i->second;
// override default wheel with selected, not very efficient, fixme
PTree opt_wheel;
if (sel_wheel.get())
{
opt_wheel.set(*sel_wheel);
opt_wheel.merge(*cfg_wheel);
cfg_wheel = &opt_wheel;
}
if (!LoadWheel(*cfg_wheel, loadDrawable, topnode, error_output))
{
error_output << "Failed to load wheels." << std::endl;
return false;
}
}
// load drawables
LoadBody loadBody(topnode, bodynode, loadDrawable);
for (PTree::const_iterator i = cfg.begin(); i != cfg.end(); ++i)
{
if (i->first != "body" &&
i->first != "steering" &&
i->first != "light-brake" &&
i->first != "light-reverse")
{
loadBody(i->second);
}
}
// load steering wheel
const PTree * cfg_steer;
if (cfg.get("steering", cfg_steer))
{
SceneNode & bodynoderef = topnode.GetNode(bodynode);
if (!loadDrawable(*cfg_steer, bodynoderef, &steernode, 0))
{
error_output << "Failed to load steering wheel." << std::endl;
return false;
}
cfg_steer->get("max-angle", steer_angle_max);
steer_angle_max = steer_angle_max / 180.0 * M_PI;
SceneNode & steernoderef = bodynoderef.GetNode(steernode);
steer_orientation = steernoderef.GetTransform().GetRotation();
steer_rotation = steer_orientation;
}
// load brake/reverse light point light sources (optional)
int i = 0;
std::string istr = "0";
const PTree * cfg_light;
while (cfg.get("light-brake-"+istr, cfg_light))
{
if (!LoadLight(*cfg_light, content, error_output))
{
error_output << "Failed to load lights." << std::endl;
return false;
}
std::ostringstream sstr;
sstr << ++i;
istr = sstr.str();
}
i = 0;
istr = "0";
while (cfg.get("light-reverse-"+istr, cfg_light))
{
if (!LoadLight(*cfg_light, content, error_output))
{
error_output << "Failed to load lights." << std::endl;
return false;
}
std::ostringstream sstr;
sstr << ++i;
istr = sstr.str();
}
// load car brake/reverse graphics (optional)
if (cfg.get("light-brake", cfg_light))
{
SceneNode & bodynoderef = topnode.GetNode(bodynode);
if (!loadDrawable(*cfg_light, bodynoderef, 0, &brakelights))
{
error_output << "Failed to load lights." << std::endl;
return false;
}
}
if (cfg.get("light-reverse", cfg_light))
{
SceneNode & bodynoderef = topnode.GetNode(bodynode);
if (!loadDrawable(*cfg_light, bodynoderef, 0, &reverselights))
{
error_output << "Failed to load lights." << std::endl;
return false;
}
}
if (!LoadCameras(cfg, camerabounce, error_output))
{
return false;
}
SetColor(carcolor[0], carcolor[1], carcolor[2]);
loaded = true;
return true;
}
void InitGui() {
InitTrackerGui(gui_vars, window_width, window_height, image_width,
image_height, rig.cameras_.size());
// std::cerr << "Viewport: " << gui_vars.camera_view->v.l << " " <<
// gui_vars.camera_view->v.r() << " " <<
// gui_vars.camera_view->v.b << " " <<
// gui_vars.camera_view->v.t() << std::endl;
pangolin::RegisterKeyPressCallback(
pangolin::PANGO_SPECIAL + pangolin::PANGO_KEY_RIGHT,
[&]() { is_stepping = true; });
pangolin::RegisterKeyPressCallback(pangolin::PANGO_CTRL + 'r', [&]() {
camera_img.reset();
is_keyframe = true;
is_prev_keyframe = true;
is_running = false;
InitTracker();
poses.clear();
gui_vars.scene_graph.Clear();
gui_vars.scene_graph.AddChild(&gui_vars.grid);
axes.clear();
LoadCameras();
prev_delta_t_ba = Sophus::SE3d();
prev_t_ba = Sophus::SE3d();
last_t_ba = Sophus::SE3d();
});
pangolin::RegisterKeyPressCallback(pangolin::PANGO_CTRL + 's', [&]() {
// write all the poses to a file.
std::ofstream pose_file("poses.txt", std::ios_base::trunc);
for (auto pose : poses) {
pose_file << pose->t_wp.translation().transpose().format(
sdtrack::kLongCsvFmt) << std::endl;
}
});
pangolin::RegisterKeyPressCallback(' ', [&]() { is_running = !is_running; });
pangolin::RegisterKeyPressCallback('b', [&]() {
// last_optimization_level = 0;
tracker.OptimizeTracks(last_optimization_level, optimize_landmarks,
optimize_pose);
UpdateCurrentPose();
});
pangolin::RegisterKeyPressCallback('B', [&]() {
do_bundle_adjustment = !do_bundle_adjustment;
std::cerr << "Do BA:" << do_bundle_adjustment << std::endl;
});
pangolin::RegisterKeyPressCallback('S', [&]() {
do_start_new_landmarks = !do_start_new_landmarks;
std::cerr << "Do SNL:" << do_start_new_landmarks << std::endl;
});
pangolin::RegisterKeyPressCallback('2',
[&]() { last_optimization_level = 2; });
pangolin::RegisterKeyPressCallback('3',
[&]() { last_optimization_level = 3; });
pangolin::RegisterKeyPressCallback('1',
[&]() { last_optimization_level = 1; });
pangolin::RegisterKeyPressCallback('0',
[&]() { last_optimization_level = 0; });
pangolin::RegisterKeyPressCallback('9', [&]() {
last_optimization_level = 0;
tracker.OptimizeTracks(-1, optimize_landmarks, optimize_pose);
UpdateCurrentPose();
});
pangolin::RegisterKeyPressCallback('p', [&]() {
tracker.PruneTracks();
// Update the pose t_ab based on the result from the tracker.
UpdateCurrentPose();
BaAndStartNewLandmarks();
});
pangolin::RegisterKeyPressCallback('l', [&]() {
optimize_landmarks = !optimize_landmarks;
std::cerr << "optimize landmarks: " << optimize_landmarks << std::endl;
});
pangolin::RegisterKeyPressCallback('c', [&]() {
optimize_pose = !optimize_pose;
std::cerr << "optimize pose: " << optimize_pose << std::endl;
});
pangolin::RegisterKeyPressCallback('m', [&]() {
is_manual_mode = !is_manual_mode;
std::cerr << "Manual mode:" << is_manual_mode << std::endl;
});
pangolin::RegisterKeyPressCallback('k', [&]() {
sdtrack::AlignmentOptions options;
options.apply_to_kp = true;
tracker.Do2dAlignment(options, tracker.GetImagePyramid(),
tracker.GetCurrentTracks(), last_optimization_level);
});
}
//
// Init GL
//
int InitGL(GLvoid) // All Setup For OpenGL Goes Here
{
// seed rand generator
srand(getclock());
#if 0
QueryPerformanceFrequency(&timerFrequency);
QueryPerformanceCounter(&lastTime);
#endif
// use the mouse only
// for camera movement
// disable the cursor
ShowCursor(FALSE);
Super_LoadGlobals();
Load_ConfigFile();
// Now load main variables
Super_LoadBots();
Super_FireAnts();
Create_Col_List();
Create_Wall_List();
// enable all the goodies
//glEnable(GL_TEXTURE_2D); // enable texture mapping
glShadeModel(GL_SMOOTH); // Enable Smooth Shading
glClearColor(0.0f, 0.0f, 0.0f, 0.5f); // Black Background
glClearDepth(1.0f); // Depth Buffer Setup
glEnable(GL_NORMALIZE);
glPolygonMode(GL_FRONT_AND_BACK, GL_FILL);
glShadeModel(GL_SMOOTH);
glEnable(GL_DEPTH_TEST); // Enables Depth Testing
glDepthFunc(GL_LEQUAL); // The Type Of Depth Testing To Do
glHint(GL_PERSPECTIVE_CORRECTION_HINT, GL_NICEST); // Really Nice Perspective Calculations
// have to load the quadrice to be used
quadric=gluNewQuadric(); // Create A Pointer To The Quadric Object (Return 0 If No Memory) (NEW)
gluQuadricNormals(quadric, GLU_SMOOTH); // Create Smooth Normals (NEW)
gluQuadricTexture(quadric, GL_TRUE); // Create Texture Coords (NEW)
#if ENABLE_LIGHTS
// InitMaterial();
#endif
GenerateBots();
GenerateFireAnts(); // a new breed of warrior
CreateWorld();
LoadCameras();
BuildFont(); // Build The Font
// Load the objects
InitObjects();
GenerateLights();
InitGlobals(); // for printing data, etc
// generate the nest objects
nest.generate();
garden.generate();
trail_set.generate();
// give the ant food
InitFood();
Build_ParticleSet();
Reset_FontID();
// load the title screen text
Load_Titles();
// the text library
Super_MainText();
CreateWalls();
//
// Load the collision test for the wall
// very easy test
// 4 different walls
//
// front wall
InsertColSegment(world_ptr->x_min, world_ptr->y_max,
world_ptr->x_max, world_ptr->y_max);
// right wall
InsertColSegment(world_ptr->x_max, world_ptr->y_min,
world_ptr->x_max, world_ptr->y_max);
// back wall (top)
InsertColSegment(world_ptr->x_min, world_ptr->y_min,
world_ptr->x_max, world_ptr->y_min);
// left wall
InsertColSegment(world_ptr->x_min, world_ptr->y_min,
world_ptr->x_min, world_ptr->y_max);
// end insertion --
//
// The first thing to do is begin in paused mode
//
Super_BeginPaused();
mSuper_Loaded = LOADED_TRUE;
return TRUE; // Initialization Went OK
} // end of the function
