diff --git a/examples/TriangulationLOSTExample.cpp b/examples/TriangulationLOSTExample.cpp new file mode 100644 index 000000000..a903c625a --- /dev/null +++ b/examples/TriangulationLOSTExample.cpp @@ -0,0 +1,161 @@ +/* ---------------------------------------------------------------------------- + + * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * Atlanta, Georgia 30332-0415 + * All Rights Reserved + * Authors: Frank Dellaert, et al. (see THANKS for the full author list) + + * See LICENSE for the license information + + * -------------------------------------------------------------------------- */ + +/** + * @file TriangulationLOSTExample.cpp + * @author Akshay Krishnan + * @brief This example runs triangulation several times using 3 different + * approaches: LOST, DLT, and DLT with optimization. It reports the covariance + * and the runtime for each approach. + * + * @date 2022-07-10 + */ +#include +#include +#include +#include +#include +#include +#include + +#include +#include +#include + +using namespace std; +using namespace gtsam; + +static std::mt19937 rng(42); + +void PrintCovarianceStats(const Matrix& mat, const std::string& method) { + Matrix centered = mat.rowwise() - mat.colwise().mean(); + Matrix cov = (centered.adjoint() * centered) / double(mat.rows() - 1); + std::cout << method << " covariance: " << std::endl; + std::cout << cov << std::endl; + std::cout << "Trace sqrt: " << sqrt(cov.trace()) << std::endl << std::endl; +} + +void PrintDuration(const std::chrono::nanoseconds dur, double num_samples, + const std::string& method) { + double nanoseconds = dur.count() / num_samples; + std::cout << "Time taken by " << method << ": " << nanoseconds * 1e-3 + << std::endl; +} + +void GetLargeCamerasDataset(CameraSet>* cameras, + std::vector* poses, Point3* point, + Point2Vector* measurements) { + const double min_xy = -10, max_xy = 10; + const double min_z = -20, max_z = 0; + const int num_cameras = 500; + cameras->reserve(num_cameras); + poses->reserve(num_cameras); + measurements->reserve(num_cameras); + *point = Point3(0.0, 0.0, 10.0); + + std::uniform_real_distribution rand_xy(min_xy, max_xy); + std::uniform_real_distribution rand_z(min_z, max_z); + Cal3_S2 identity_K; + + for (int i = 0; i < num_cameras; ++i) { + Point3 wti(rand_xy(rng), rand_xy(rng), rand_z(rng)); + Pose3 wTi(Rot3(), wti); + + poses->push_back(wTi); + cameras->emplace_back(wTi, identity_K); + measurements->push_back(cameras->back().project(*point)); + } +} + +void GetSmallCamerasDataset(CameraSet>* cameras, + std::vector* poses, Point3* point, + Point2Vector* measurements) { + Pose3 pose_1; + Pose3 pose_2(Rot3(), Point3(5., 0., -5.)); + Cal3_S2 identity_K; + PinholeCamera camera_1(pose_1, identity_K); + PinholeCamera camera_2(pose_2, identity_K); + + *point = Point3(0, 0, 1); + cameras->push_back(camera_1); + cameras->push_back(camera_2); + *poses = {pose_1, pose_2}; + *measurements = {camera_1.project(*point), camera_2.project(*point)}; +} + +Point2Vector AddNoiseToMeasurements(const Point2Vector& measurements, + const double measurement_sigma) { + std::normal_distribution normal(0.0, measurement_sigma); + + Point2Vector noisy_measurements; + noisy_measurements.reserve(measurements.size()); + for (const auto& p : measurements) { + noisy_measurements.emplace_back(p.x() + normal(rng), p.y() + normal(rng)); + } + return noisy_measurements; +} + +/* ************************************************************************* */ +int main(int argc, char* argv[]) { + CameraSet> cameras; + std::vector poses; + Point3 gt_point; + Point2Vector measurements; + GetLargeCamerasDataset(&cameras, &poses, >_point, &measurements); + // GetSmallCamerasDataset(&cameras, &poses, >_point, &measurements); + const double measurement_sigma = 1e-2; + SharedNoiseModel measurement_noise = + noiseModel::Isotropic::Sigma(2, measurement_sigma); + + const long int num_trials = 1000; + Matrix dlt_errors = Matrix::Zero(num_trials, 3); + Matrix lost_errors = Matrix::Zero(num_trials, 3); + Matrix dlt_opt_errors = Matrix::Zero(num_trials, 3); + + double rank_tol = 1e-9; + boost::shared_ptr calib = boost::make_shared(); + std::chrono::nanoseconds dlt_duration; + std::chrono::nanoseconds dlt_opt_duration; + std::chrono::nanoseconds lost_duration; + std::chrono::nanoseconds lost_tls_duration; + + for (int i = 0; i < num_trials; i++) { + Point2Vector noisy_measurements = + AddNoiseToMeasurements(measurements, measurement_sigma); + + auto lost_start = std::chrono::high_resolution_clock::now(); + boost::optional estimate_lost = triangulatePoint3( + cameras, noisy_measurements, rank_tol, false, measurement_noise, true); + lost_duration += std::chrono::high_resolution_clock::now() - lost_start; + + auto dlt_start = std::chrono::high_resolution_clock::now(); + boost::optional estimate_dlt = triangulatePoint3( + cameras, noisy_measurements, rank_tol, false, measurement_noise, false); + dlt_duration += std::chrono::high_resolution_clock::now() - dlt_start; + + auto dlt_opt_start = std::chrono::high_resolution_clock::now(); + boost::optional estimate_dlt_opt = triangulatePoint3( + cameras, noisy_measurements, rank_tol, true, measurement_noise, false); + dlt_opt_duration += + std::chrono::high_resolution_clock::now() - dlt_opt_start; + + lost_errors.row(i) = *estimate_lost - gt_point; + dlt_errors.row(i) = *estimate_dlt - gt_point; + dlt_opt_errors.row(i) = *estimate_dlt_opt - gt_point; + } + PrintCovarianceStats(lost_errors, "LOST"); + PrintCovarianceStats(dlt_errors, "DLT"); + PrintCovarianceStats(dlt_opt_errors, "DLT_OPT"); + + PrintDuration(lost_duration, num_trials, "LOST"); + PrintDuration(dlt_duration, num_trials, "DLT"); + PrintDuration(dlt_opt_duration, num_trials, "DLT_OPT"); +} \ No newline at end of file