| 452 | } |
| 453 | |
| 454 | int main(int argc, char **argv) |
| 455 | { |
| 456 | ros::init(argc, argv, "pose_graph"); |
| 457 | ros::NodeHandle n("~"); |
| 458 | posegraph.registerPub(n); |
| 459 | |
| 460 | // read param |
| 461 | n.getParam("visualization_shift_x", VISUALIZATION_SHIFT_X); |
| 462 | n.getParam("visualization_shift_y", VISUALIZATION_SHIFT_Y); |
| 463 | n.getParam("skip_cnt", SKIP_CNT); |
| 464 | n.getParam("skip_dis", SKIP_DIS); |
| 465 | std::string config_file; |
| 466 | n.getParam("config_file", config_file); |
| 467 | cv::FileStorage fsSettings(config_file, cv::FileStorage::READ); |
| 468 | if(!fsSettings.isOpened()) |
| 469 | { |
| 470 | std::cerr << "ERROR: Wrong path to settings" << std::endl; |
| 471 | } |
| 472 | |
| 473 | double camera_visual_size = fsSettings["visualize_camera_size"]; |
| 474 | cameraposevisual.setScale(camera_visual_size); |
| 475 | cameraposevisual.setLineWidth(camera_visual_size / 10.0); |
| 476 | |
| 477 | |
| 478 | LOOP_CLOSURE = fsSettings["loop_closure"]; |
| 479 | std::string IMAGE_TOPIC; |
| 480 | int LOAD_PREVIOUS_POSE_GRAPH; |
| 481 | if (LOOP_CLOSURE) |
| 482 | { |
| 483 | ROW = fsSettings["image_height"]; |
| 484 | COL = fsSettings["image_width"]; |
| 485 | std::string pkg_path = ros::package::getPath("pose_graph"); |
| 486 | string vocabulary_file = pkg_path + "/../support_files/brief_k10L6.bin"; |
| 487 | cout << "vocabulary_file" << vocabulary_file << endl; |
| 488 | posegraph.loadVocabulary(vocabulary_file); |
| 489 | |
| 490 | BRIEF_PATTERN_FILE = pkg_path + "/../support_files/brief_pattern.yml"; |
| 491 | cout << "BRIEF_PATTERN_FILE" << BRIEF_PATTERN_FILE << endl; |
| 492 | m_camera = camodocal::CameraFactory::instance()->generateCameraFromYamlFile(config_file.c_str()); |
| 493 | |
| 494 | fsSettings["image_topic"] >> IMAGE_TOPIC; |
| 495 | fsSettings["pose_graph_save_path"] >> POSE_GRAPH_SAVE_PATH; |
| 496 | fsSettings["output_path"] >> VINS_RESULT_PATH; |
| 497 | fsSettings["save_image"] >> DEBUG_IMAGE; |
| 498 | |
| 499 | // create folder if not exists |
| 500 | FileSystemHelper::createDirectoryIfNotExists(POSE_GRAPH_SAVE_PATH.c_str()); |
| 501 | FileSystemHelper::createDirectoryIfNotExists(VINS_RESULT_PATH.c_str()); |
| 502 | |
| 503 | VISUALIZE_IMU_FORWARD = fsSettings["visualize_imu_forward"]; |
| 504 | LOAD_PREVIOUS_POSE_GRAPH = fsSettings["load_previous_pose_graph"]; |
| 505 | FAST_RELOCALIZATION = fsSettings["fast_relocalization"]; |
| 506 | VINS_RESULT_PATH = VINS_RESULT_PATH + "/vins_result_loop.csv"; |
| 507 | std::ofstream fout(VINS_RESULT_PATH, std::ios::out); |
| 508 | fout.close(); |
| 509 | fsSettings.release(); |
| 510 | |
| 511 | if (LOAD_PREVIOUS_POSE_GRAPH) |
nothing calls this directly
no test coverage detected