diff --git a/cli/cli.cpp b/cli/cli.cpp index 6aefd70..19d6631 100644 --- a/cli/cli.cpp +++ b/cli/cli.cpp @@ -13,7 +13,7 @@ /* The minimum sets of parameters in CLI mode (to pass verification) are: * new + project_name + project_path + images_path + action - * open + action + * open + project_path + action * temp + images_path + action */ @@ -45,8 +45,8 @@ bool VerifyCommandlineArguments() { return false; } - // TODO(uladbohdan): to support more actions. - if (FLAGS_action != "reconstruction") { + // TODO(uladbohdan): to rename 'slam' action. + if (FLAGS_action != "reconstruction" && FLAGS_action != "slam") { LOG(ERROR) << "You must pass a valid action to proceed."; return false; } @@ -82,7 +82,7 @@ void RunCLI() { LOG(INFO) << "Running in CLI mode."; if (!VerifyCommandlineArguments()) { - LOG(ERROR) << "CL arguments verification failed."; + LOG(ERROR) << "Command Line arguments verification failed."; return; } @@ -100,11 +100,11 @@ void RunCLI() { QString temp_project_name = QString("appa-project-") + QDateTime::currentDateTime().toString(Qt::ISODate); - temp_project_path = QDir::temp().filePath(temp_project_name); + temp_project_path = "/home/parallels/tmp/" + temp_project_name; // QDir::temp().filePath(temp_project_name); LOG(INFO) << "Temp project will be created in " << temp_project_path.toStdString(); project = new Project(temp_project_name, - temp_project_path, + "/home/parallels/tmp", QString::fromStdString(FLAGS_images_path), QString::fromStdString(FLAGS_output_path)); } @@ -112,6 +112,10 @@ void RunCLI() { if (FLAGS_action == "reconstruction") { LOG(INFO) << "Starting reconstruction..."; project->BuildModelToBinary(); + } else if (FLAGS_action == "slam") { + LOG(INFO) << "Performing SLAM experiments..."; + project->Load_SLAM_data(); + project->SLAM_Build(); } if (FLAGS_cli_mode == "temp") { diff --git a/main.cpp b/main.cpp index a19f179..e23350b 100644 --- a/main.cpp +++ b/main.cpp @@ -23,7 +23,8 @@ int main(int argc, char* argv[]) { awesome->initFontAwesome(); MainWindow w; - w.SetIcons(awesome); + // Temporary avoiding rendering icons. + // w.SetIcons(awesome); w.show(); return a.exec(); diff --git a/src/io/projectio.cpp b/src/io/projectio.cpp index c448b18..5ed9155 100644 --- a/src/io/projectio.cpp +++ b/src/io/projectio.cpp @@ -1,12 +1,22 @@ // Copyright 2017 Lybros. +#include +#include +#include + #include "projectio.h" #include "../project.h" #include #include +#include #include +#include + +#include + +#include ProjectIO::ProjectIO(Project *project) : project_(project), pr(project) { } @@ -158,3 +168,183 @@ bool ProjectIO::ReadOutputLocation(QTextStream& stream) { << "Failed to initialize output location from config file."; return true; } + +bool ProjectIO::ReadCameras() { + QFile trajectoryFile("/home/parallels/co-graph/KeyFrameTrajectory.txt"); + + // TUM format: The format of each line is 'timestamp tx ty tz qx qy qz qw' + + LOG(INFO) << "Now we are reading KeyFrameTrajectory file..."; + + if (!trajectoryFile.open(QIODevice::ReadOnly)) { return false; } + + QVector& imageNames = pr->GetStorage()->GetImages(); + + QTextStream stream(&trajectoryFile); + while (!stream.atEnd()) { + double ts; + stream >> ts; // the timestamp. + double tx, ty, tz, qx, qy, qz, qw; + stream >> tx >> ty >> tz >> qx >> qy >> qz >> qw; + + // tx ty tz are world camera position. Might be enought to perform the + // experiment. + + theia::Camera* camera = new theia::Camera(); + camera->SetFocalLength(517.); + camera->SetPrincipalPoint(318.643040, 255.313989); + camera->SetImageSize(640, 480); + + camera->SetPosition(Eigen::Vector3d(tx, ty, tz)); +// LOG(INFO) << "Camera: " << camera.GetPosition() << " PUSHED."; + auto q = Eigen::Quaterniond{qx, qy, qz, qw}; + auto rotMatrix = q.normalized().toRotationMatrix(); + LOG(INFO) << rotMatrix; + camera->SetOrientationFromRotationMatrix(rotMatrix); + + pr->slam_cameras_.push_back(*camera); + if (ts > 0.0001) { +// LOG(INFO) << "TS " << ts << " segfault?"; + QString image_name = imageNames[(int)ts - 1]; + pr->slam_camera_names_.push_back(image_name); + std::string short_name = FileNameFromPath(image_name).toStdString(); + pr->slam_cameras_map_[short_name] = *camera; + LOG(INFO) << "OK so for image " << short_name << " camera POS is " + << pr->slam_cameras_map_[short_name].GetPosition(); + } + } + + // to avoid adding fake (0,0,0) camera in the end. + pr->slam_cameras_.pop_back(); + + LOG(INFO) << "SLAM CAMERAS PUSHED: " << pr->slam_cameras_.size(); +} + +bool ProjectIO::ReadSlamData() { + ReadCameras(); + + QFile slamFile("/home/parallels/co-graph/co-graph.txt"); + + LOG(INFO) << "in ProjectIO class."; + + if (!slamFile.open(QIODevice::ReadOnly)) { return false; } + + QTextStream stream(&slamFile); + QString temp_line; + + int number_of_keyframes; + + stream >> temp_line; // NUMBER_OF_KEYFRAMES + stream >> number_of_keyframes; + + LOG(INFO) << "Number of keyframes read: " << number_of_keyframes; + + std::vector kfIDs; + + // maps ID of a world point to list of KF IDs where it is observed from. + std::unordered_map> observations; + + // this maps keyframeID : worldPointID : 2d_coordinates + std::unordered_map>> image_coordinates; + + QVector& imageNames = pr->GetStorage()->GetImages(); + + for (int i = 0; i < number_of_keyframes; i++) { + stream >> temp_line; // KF + int kfId; + stream >> kfId; + stream >> temp_line; // COVISIBLE_WITH + int covisibleWith; + stream >> covisibleWith; + for (int j = 0; j < covisibleWith; j++) { + stream >> temp_line; + // Seems like I do not need this information at the moment. + } + stream >> temp_line; // NUMBER_OF_POINTS + int number_points; + stream >> number_points; +// LOG(INFO) << "number of points: " << number_points; + for (int j = 0; j < number_points; j++) { + int pointId, x, y; + stream >> pointId >> x >> y; +// LOG(INFO) << "about to update maps..."; + image_coordinates[kfId][pointId] = std::make_pair(x, y); + observations[kfId].push_back(pointId); +// LOG(INFO) << "done."; + + pr->slam_observations_[pointId].push_back( + Observation(pointId, + FileNameFromPath(imageNames[kfId - 1]).toStdString(), + x, y) + ); + } + kfIDs.push_back(kfId); + } + + for (auto it = observations.begin(); it != observations.end(); it++) { + std::sort(it->second.begin(), it->second.end()); + } + + std::sort(kfIDs.begin(), kfIDs.end()); + + LOG(INFO) << "all sorted."; + + int LARGE_CONST = 10000; + int MIN_SIZE_THRESHOLD = 10; + + for (int i = 0; i < kfIDs.size(); i++) { + for (int j = i+1; j < kfIDs.size(); j++) { + int kf1 = kfIDs[i]; +// LOG(INFO) << "First point vector: kf1=" << kf1; +// LOG(INFO) << "Size: " << observations[kf1].size(); +// for (auto it = observations[kf1].begin(); it != observations[kf1].end(); it++) { +// LOG(INFO) << *it; +// } + int kf2 = kfIDs[j]; + int pairID = kf1 * LARGE_CONST + kf2; + std::vector merged(2000); + std::vector::iterator it = std::set_intersection( + observations[kf1].begin(), observations[kf1].end(), + observations[kf2].begin(), observations[kf2].end(), merged.begin()); + merged.resize(it-merged.begin()); + +// LOG(INFO) << kf1 << ":" << kf2 << " merged."; + + if (merged.size() < MIN_SIZE_THRESHOLD) { +// LOG(INFO) << kf1 << ":" << kf2 << " MERGED SIZE TOO SMALL " << merged.size(); + continue; + } + + theia::ImagePairMatch pMatch; + + for (int k = 0; k < merged.size(); k++) { +// LOG(INFO) << "about to push..."; + pMatch.correspondences.push_back( + theia::FeatureCorrespondence( + theia::Feature(image_coordinates[kf1][merged[k]].first, + image_coordinates[kf1][merged[k]].second), + theia::Feature(image_coordinates[kf2][merged[k]].first, + image_coordinates[kf2][merged[k]].second)) + ); +// LOG(INFO) << "done."; + } +// LOG(INFO) << "???"; + + pMatch.image1 = imageNames[kf1 - 1].toStdString(); + pMatch.image2 = imageNames[kf2 - 1].toStdString(); + + if (pMatch.image1.length() > 1 && pMatch.image2.length() > 1) { + pr->slam_data_[pairID] = pMatch; +// LOG(INFO) << "INFO PUSHED FOR " << kf1 << ":" << kf2 +// << " which are " << pMatch.image1 << " " << pMatch.image2 +// << " matches: " << merged.size() << endl; + } else { + LOG(INFO) << " #### ALERT BROKEN NAMES"; + } + } + } + + slamFile.close(); + + return true; +} diff --git a/src/io/projectio.h b/src/io/projectio.h index e6f4179..22001ac 100644 --- a/src/io/projectio.h +++ b/src/io/projectio.h @@ -44,6 +44,9 @@ class ProjectIO { bool WriteConfigurationFile(); bool ReadConfigurationFile(); + bool ReadCameras(); + bool ReadSlamData(); + ~ProjectIO(); private: diff --git a/src/project.cpp b/src/project.cpp index 7bfc600..5ab6276 100644 --- a/src/project.cpp +++ b/src/project.cpp @@ -111,6 +111,34 @@ QSet* Project::SearchImage( return found_tracks; } +void Project::Load_SLAM_data() { + LOG(INFO) << "About to start with SLAM experiments."; + CHECK(ReadSlamData()) << "Failed to read Experimental SLAM data."; + + LOG(INFO) << "SLAM DATA size " << slam_data_.size(); + + LOG(INFO) << "181 data: " << slam_data_[181*10000+184].correspondences.size(); + + LOG(INFO) << "SLAM Camera Names size: " << slam_camera_names_.size(); + +// LOG(INFO) << "====\n" << "Starting with reconstruction...\n" +// << "====\n"; + + // Now building a subset of images. + storage_->KeepImagesSubset(slam_camera_names_); + + LOG(INFO) << "Subset size:" << storage_->GetSlamImages().size(); +} + +void Project::SLAM_Build() { + Reconstructor(this).FastBuild(); +// Reconstructor(this).SmartBuild(); +} + +void Project::SLAM_New_Build() { + Reconstructor(this).SLAM_New_Build(); +} + void Project::ExtractFeatures() { features_->ForceExtract(); } @@ -153,6 +181,10 @@ bool Project::ReadConfigurationFile() { return ProjectIO(this).ReadConfigurationFile(); } +bool Project::ReadSlamData() { + return ProjectIO(this).ReadSlamData(); +} + QString Project::GetConfigurationFilePath() { return QDir(project_path_).filePath(CONFIG_FILE_NAME); } diff --git a/src/project.h b/src/project.h index e3d9357..90eab6d 100644 --- a/src/project.h +++ b/src/project.h @@ -4,7 +4,7 @@ * project_name_folder * |--"project-config" * - * More about the structure in io/ firectory. + * More about the structure in io/ directory. */ #ifndef SRC_PROJECT_H_ @@ -13,6 +13,7 @@ #include #include #include +#include #include #include @@ -22,6 +23,8 @@ #include #include +#include +#include #include "io/projectio.h" #include "featuresx.h" @@ -33,6 +36,14 @@ const QString CONFIG_FILE_NAME = "project-config"; const QString DEFAULT_OUTPUT_LOCATION_POSTFIX = "out/"; const QString DEFAULT_MODEL_BINARY_FILENAME = "model-0.binary"; +struct Observation { + Observation(int pId, std::string filename, int x, int y) : worldPointId(pId), filename(filename), x(x), y(y) {} + + int worldPointId; + std::string filename; + int x, y; +}; + class Project { friend class ProjectIO; @@ -78,6 +89,8 @@ class Project { bool ReadConfigurationFile(); + bool ReadSlamData(); + QString GetDefaultOutputPath(); Storage* GetStorage(); @@ -86,6 +99,31 @@ class Project { ~Project(); + void Load_SLAM_data(); + // The method reads data generated by some SLAM engine and runs reconstruction + // process. + // TODO(uladbohdan): to rename once there's an understanding what are we + // doing here. + void SLAM_Build(); + + void SLAM_New_Build(); + + std::unordered_map& GetSlamData() { + return slam_data_; + } + + std::vector& GetSLAMCameras() { + return slam_cameras_; + } + + std::unordered_map& GetCamerasMap() { + return slam_cameras_map_; + } + + std::unordered_map> GetSlamObservations() { + return slam_observations_; + } + private: Options* options_; @@ -98,6 +136,20 @@ class Project { Features* features_; QString GetConfigurationFilePath(); + + // TODO(uladbohdan): to consider moving it somewhere else. + // + // The key is a two ORB_SLAM2 timestamps merged. ImagePairMatch gathers all + // the data about these two images combination. + std::unordered_map slam_data_; + + std::vector slam_cameras_; + std::unordered_map slam_cameras_map_; + QVector slam_camera_names_; + + // This is to push observations into reconstruction. + // Maps PointId to . + std::unordered_map> slam_observations_; }; #endif // SRC_PROJECT_H_ diff --git a/src/reconstructor.cpp b/src/reconstructor.cpp index 61fa505..3e314e0 100644 --- a/src/reconstructor.cpp +++ b/src/reconstructor.cpp @@ -10,6 +10,12 @@ #include "io/storageio.h" +using theia::ReconstructionEstimator; +using theia::ReconstructionEstimatorOptions; +using theia::ReconstructionEstimatorSummary; + +using theia::TwoViewInfoFromTwoCameras; + Reconstructor::Reconstructor(Project* project) : project_(project) { storage_ = project_->GetStorage(); options_ = project_->GetOptions(); @@ -30,6 +36,13 @@ void Reconstructor::SmartBuild() { report_->using_prebuilt_matches_ = true; +// if (!storage_->GetSlamImages().empty()) { +// LOG(INFO) << "loading SLAM subset into the Reconstructor..."; +// if (!ExtractFeaturesMatches(&reconstruction_builder)) { +// LOG(ERROR) << "Failed to extract features and matches."; +// return; +// } +// } else { if (!ReadMatches(&reconstruction_builder)) { LOG(INFO) << "Failed to read matches. Starting the process from scratch..."; report_->using_prebuilt_matches_ = false; @@ -38,6 +51,7 @@ void Reconstructor::SmartBuild() { return; } } +// } LOG(INFO) << "Starting reconstruction process..."; @@ -224,3 +238,213 @@ bool Reconstructor::ExtractFeaturesMatches( LOG(INFO) << "Extracted and matched successfully!"; return true; } + +void Reconstructor::FastBuild() { + LOG(INFO) << "About to perform Fast Reconstruction..."; +// theia::Timer overall_timer; + +// ReconstructionBuilderOptions options = +// options_->GetReconstructionBuilderOptions(); + +// ReconstructionBuilder reconstruction_builder(options); + + LOG(INFO) << "Number of images for reconstruction: " + << storage_->GetProperImages().size(); + +// theia::CameraIntrinsicsGroupId intrinsics_group_id = +// theia::kInvalidCameraIntrinsicsGroupId; + + CameraIntrinsicsPrior prior; + prior.image_height = 480; + prior.image_width = 640; + prior.focal_length.is_set = true; + prior.focal_length.value[0] = 517.; + prior.principal_point.is_set = true; + prior.principal_point.value[0] = 318.6; + prior.principal_point.value[1] = 255.3; + +// LOG(INFO) << "About to push images?.."; + +// for (QString image_path : storage_->GetProperImages()) { +// auto im = FileNameFromPath(image_path).toStdString(); +// LOG(INFO) << "Adding " << im; +// CHECK(reconstruction_builder.AddImageWithCameraIntrinsicsPrior( +// im, prior, 0)) +// << "Failed to add image! :("; +// } + + auto slam_data = project_->GetSlamData(); + + ReconstructionEstimatorOptions estimator_options; +// estimator_options.min_num_two_view_inliers = 10; + + theia::ReconstructionEstimator* estimator = + ReconstructionEstimator::Create(estimator_options); + + auto cameras_map = project_->GetCamerasMap(); + + theia::ViewGraph* view_graph = new theia::ViewGraph(); + theia::Reconstruction* reconstruction = new theia::Reconstruction(); + + LOG(INFO) << "Adding Views to the Reconstruction..."; + for (QString name : storage_->GetSlamImages()) { + std::string short_name = FileNameFromPath(name).toStdString(); + LOG(INFO) << "Adding a view: " << short_name; + reconstruction->AddView(short_name); + } + LOG(INFO) << "View IDs are now usable."; + + for (auto it = slam_data.begin(); it != slam_data.end(); it++) { + QString im1 = FileNameFromPath(QString::fromStdString(it->second.image1)); +// LOG(INFO) << " - " << it->second.image1 << " -> " << im1.toStdString(); + QString im2 = FileNameFromPath(QString::fromStdString(it->second.image2)); +// LOG(INFO) << " - " << it->second.image2 << " -> " << im2.toStdString(); + if (im1.length() > 1 && im2.length() > 1 && + it->second.correspondences.size() > estimator_options.min_num_two_view_inliers) { + LOG(INFO) << "Correspondences: " << it->second.correspondences.size(); +// reconstruction_builder.AddTwoViewMatch( +// im1.toStdString(), im2.toStdString(), it->second); +// } + theia::TwoViewInfo* two_view_info = new theia::TwoViewInfo(); + theia::Camera cam1 = cameras_map[im1.toStdString()]; + theia::Camera cam2 = cameras_map[im2.toStdString()]; +// LOG(INFO) << "Camera 1 POS " << cam1.GetPosition(); +// LOG(INFO) << "Camera 1 ORI " << cam2.GetOrientationAsRotationMatrix(); + TwoViewInfoFromTwoCameras(cam1, cam2, two_view_info); + two_view_info->num_verified_matches = it->second.correspondences.size(); + +// LOG(INFO) << "Was two_view_info populated? " << two_view_info->num_verified_matches << "\n" +// << two_view_info->num_homography_inliers << "\n" +// << two_view_info->rotation_2 << "\n" << two_view_info->position_2; + + view_graph->AddEdge(reconstruction->ViewIdFromName(im1.toStdString()), + reconstruction->ViewIdFromName(im2.toStdString()), + *two_view_info); + } + } + + // One more approaching: pushing SLAM observations into reconstruction. + auto observations = project_->GetSlamObservations(); + int c1 = 0, c2 = 0; + for(auto obs : observations) { + c1++; + theia::TrackId tId = reconstruction->AddTrack(); + for (Observation oneObs : obs.second) { + theia::ViewId vId = reconstruction->ViewIdFromName(oneObs.filename); + reconstruction->AddObservation(vId, tId, theia::Feature(oneObs.x, oneObs.y)); + c2++; + } + } + LOG(INFO) << "OBSERVATIONS STATS: tracks: " << c1 << " observations: " << c2; + + LOG(INFO) << "Launching Estimator!"; + + LOG(INFO) << "Views in reconstruction: " << reconstruction->NumViews(); + + LOG(INFO) << "Num of edges:" << view_graph->GetAllEdges().size(); + + ReconstructionEstimatorSummary summary = estimator->Estimate(view_graph, reconstruction); + LOG(INFO) << "Got summary. TRACKS (= 3D points):" << summary.estimated_tracks.size(); + LOG(INFO) << "VIEWS:" << summary.estimated_views.size(); + LOG(INFO) << "SUCCESS " << summary.success; + LOG(INFO) << "Total time:" << summary.total_time; + LOG(INFO) << "MESSAGE:" << summary.message; + +// report_->using_prebuilt_matches_ = true; + + /* if (!ReadMatches(&reconstruction_builder)) { + LOG(INFO) << "Failed to read matches. Starting the process from scratch..."; + report_->using_prebuilt_matches_ = false; + if (!ExtractFeaturesMatches(&reconstruction_builder)) { + LOG(ERROR) << "Failed to extract features and matches."; + return; + } + } + + LOG(INFO) << "Starting reconstruction process...";*/ + +// theia::Timer reconstructing_timer; + + // !!! +// std::vector reconstructions; +// CHECK(reconstruction_builder.BuildReconstruction(&reconstructions)) +// << "Could not create a reconstruction"; + + +// report_->reconstruction_time_ = reconstructing_timer.ElapsedTimeInSeconds(); + +// LOG(INFO) << "Reconstruction has been created."; + + // Using raw image data to colorize the reconstructions. + // The method is running using 2 threads. +// theia::Timer colorizing_timer; +// for (int i = 0; i < reconstructions.size(); i++) { +// theia::ColorizeReconstruction(project_->GetImagesPath().toStdString(), 2, +// reconstructions[i]); +// } +// report_->colorizing_time_ = colorizing_timer.ElapsedTimeInSeconds(); +// LOG(INFO) << "Reconstruction colorized successfully!"; + + std::vector reconstructions; + reconstructions.push_back(reconstruction); + + StorageIO(storage_).WriteReconstructions(reconstructions); + +// report_->overall_time_ = overall_timer.ElapsedTimeInSeconds(); + +// // Generating a report. +// QString report_path = report_->GetDefaultReportPath(); +// bool ok = report_->GenerateSmartReconstructionReport(report_path); +// if (ok) { +// LOG(INFO) << "Report was successfully created. Written to " +// << report_path.toStdString(); +// } else { +// LOG(INFO) << "Failed to create reconstruction report."; +// } + + return; +} + +void Reconstructor::SLAM_New_Build() { + ReconstructionBuilderOptions options = + options_->GetReconstructionBuilderOptions(); + + ReconstructionBuilder reconstruction_builder(options); + + CameraIntrinsicsPrior prior; + prior.image_height = 480; + prior.image_width = 640; + prior.focal_length.is_set = true; + prior.focal_length.value[0] = 517.; + prior.principal_point.is_set = true; + prior.principal_point.value[0] = 318.6; + prior.principal_point.value[1] = 255.3; + + for (QString name : storage_->GetSlamImages()) { +// std::string short_name = FileNameFromPath(name).toStdString(); + reconstruction_builder.AddImageWithCameraIntrinsicsPrior(name.toStdString(), prior, 1); + LOG(INFO) << "!! " << name.toStdString(); + } + LOG(INFO) << "A total of " << storage_->GetSlamImages().size() << " added."; + + std::unordered_map& matches = project_->GetSlamData(); + LOG(INFO) << "XXX " << matches.size(); + for (auto match : matches) { + if (match.second.image1.length() < 2 || match.second.image2.length() < 2) { + continue; + } + LOG(INFO) << "## " << match.second.image1 << " " << match.second.image2; + std::string im1, im2; + CHECK(theia::GetFilenameFromFilepath(match.second.image1, true, &im1)); + CHECK(theia::GetFilenameFromFilepath(match.second.image2, true, &im2)); +// std::string im2 = FileNameFromPath(match.second.image2); + reconstruction_builder.AddTwoViewMatch(im1, im2, + match.second); + } + + std::vector reconstructions; + CHECK(reconstruction_builder.BuildReconstruction(&reconstructions)) + << "Could not create a reconstruction"; + + StorageIO(storage_).WriteReconstructions(reconstructions); +} diff --git a/src/reconstructor.h b/src/reconstructor.h index eda46d7..6026b78 100644 --- a/src/reconstructor.h +++ b/src/reconstructor.h @@ -24,6 +24,11 @@ class Reconstructor { void SmartBuild(); + // No feature extraction, no matching. + void FastBuild(); + + void SLAM_New_Build(); + private: Project* project_; Storage* storage_; diff --git a/src/storage.cpp b/src/storage.cpp index 521e5b9..bf367a0 100644 --- a/src/storage.cpp +++ b/src/storage.cpp @@ -7,6 +7,7 @@ Storage::Storage() : options_(nullptr), images_(nullptr), images_path_(""), + slam_reduced_images_(nullptr), output_location_(""), loaded_reconstruction_(nullptr), loaded_reconstruction_name_("") { @@ -178,5 +179,6 @@ QString Storage::GetCameraIntrinsicsPath() const { Storage::~Storage() { delete images_; + delete slam_reduced_images_; delete loaded_reconstruction_; } diff --git a/src/storage.h b/src/storage.h index 0bad171..1596695 100644 --- a/src/storage.h +++ b/src/storage.h @@ -71,6 +71,27 @@ class Storage { void LoadModelsList(); + void KeepImagesSubset(QVector& subset) { + // TO IMPROVE. + slam_reduced_images_ = new QVector(subset); + + + /*slam_reduced_images_->resize(subset.size()); + for (int i = 0; i < subset.size(); i++) { + (*slam_reduced_images_)[i] = subset[i]; + }*/ + + LOG(INFO) << "Alternative subset created with size: " << slam_reduced_images_->length(); + } + + QVector& GetSlamImages() { + return *slam_reduced_images_; + } + + QVector& GetProperImages() { + return slam_reduced_images_->empty() ? *images_ : *slam_reduced_images_; + } + ~Storage(); private: @@ -85,6 +106,8 @@ class Storage { QString images_path_; QString output_location_; + QVector* slam_reduced_images_; + Options* options_; // Reads one specific Reconstruction by name from filesystem. diff --git a/ui/mainwindow.cpp b/ui/mainwindow.cpp index ba2de61..daf8de9 100644 --- a/ui/mainwindow.cpp +++ b/ui/mainwindow.cpp @@ -352,3 +352,24 @@ theia::Reconstruction* MainWindow::SelectModel() { return active_project_->GetStorage()->GetReconstruction( full_names[short_names.indexOf(QRegExp(reconstruction_name))]); } + +void MainWindow::on_actionRender_SLAM_Cameras_triggered() { + active_project_->Load_SLAM_data(); + LoadImagesPreview(); + LOG(INFO) << "previews updated."; + view_->RenderSlamExperiment(active_project_->GetSLAMCameras()); +} + +void MainWindow::on_actionLoad_SLAM_triggered() { + active_project_->Load_SLAM_data(); + LOG(INFO) << "SLAM files loaded into the App!"; +} + +void MainWindow::on_actionBuild_with_SLAM_triggered() { + LOG(INFO) << "About to perform SLAM build..."; + active_project_->SLAM_Build(); +} + +void MainWindow::on_actionNew_Build_triggered() { + active_project_->SLAM_New_Build(); +} diff --git a/ui/mainwindow.h b/ui/mainwindow.h index 8c53651..b83f28b 100644 --- a/ui/mainwindow.h +++ b/ui/mainwindow.h @@ -61,7 +61,15 @@ class MainWindow : public QMainWindow { void on_actionSearch_Image_triggered(); - private: + void on_actionRender_SLAM_Cameras_triggered(); + + void on_actionLoad_SLAM_triggered(); + + void on_actionBuild_with_SLAM_triggered(); + + void on_actionNew_Build_triggered(); + +private: Ui::MainWindow* ui; Project* active_project_; diff --git a/ui/mainwindow.ui b/ui/mainwindow.ui index 7e24a1d..0852c40 100644 --- a/ui/mainwindow.ui +++ b/ui/mainwindow.ui @@ -11,7 +11,7 @@ - Appa + Surface Reconstruction @@ -319,6 +319,10 @@ false + + + + @@ -419,7 +423,7 @@ false - Visualize a binary (default - out/model-0.binary) + Visualize binary Visualize a binary (default - out/model-0.binary) @@ -436,6 +440,38 @@ Run Theia's view_reconstruction binary + + + Render SLAM Cameras + + + This only renders cameras from co-graph/KeyTrajectory file + + + + + Load SLAM data + + + Load SLAM data into Memory and that is it. + + + + + Fast Build + + + Assuming SLAM data is already loaded, building the project. + + + + + New Build + + + using SLAM_Build() method + + diff --git a/ui/reconstruction_window.cpp b/ui/reconstruction_window.cpp index a7eb57b..912431b 100644 --- a/ui/reconstruction_window.cpp +++ b/ui/reconstruction_window.cpp @@ -227,5 +227,26 @@ void ReconstructionWindow::SetSelectedPoints( } } +void ReconstructionWindow::RenderSlamExperiment( + std::vector& cameras) { + cameras_.clear(); + + cameras_.reserve(cameras.size()); + + for (const theia::Camera c : cameras) { + theia::Camera cc; + cc.SetPosition(c.GetPosition() * 100); + cc.SetOrientationFromRotationMatrix(c.GetOrientationAsRotationMatrix()); + LOG(INFO) << "ROTATION: " << cc.GetOrientationAsRotationMatrix(); + cameras_.emplace_back(ModifiedCamera(cc)); + } + + LOG(INFO) << "Cameras pushed for rendering: " << cameras_.size(); + + draw(); + + update(); +} + ReconstructionWindow::~ReconstructionWindow() { } diff --git a/ui/reconstruction_window.h b/ui/reconstruction_window.h index aa65478..a2ca298 100644 --- a/ui/reconstruction_window.h +++ b/ui/reconstruction_window.h @@ -71,6 +71,8 @@ class ReconstructionWindow : public QGLViewer { // Set selected flag for WorldPoint void SetSelectedPoints(const std::multiset& s_tracks); + void RenderSlamExperiment(std::vector& cameras); + ~ReconstructionWindow(); protected: