/* ---------------------------------------------------------------------------- * 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"); }