From 897dae4436e9d8ce5b9518c23cca7abe9e5b7f4b Mon Sep 17 00:00:00 2001 From: Akshay Krishnan Date: Tue, 12 Jul 2022 20:03:02 -0700 Subject: [PATCH] more renames to camelcase --- examples/TriangulationLOSTExample.cpp | 130 +++++++++++++------------- gtsam/geometry/triangulation.h | 16 ++-- 2 files changed, 72 insertions(+), 74 deletions(-) diff --git a/examples/TriangulationLOSTExample.cpp b/examples/TriangulationLOSTExample.cpp index a903c625a..a862026e0 100644 --- a/examples/TriangulationLOSTExample.cpp +++ b/examples/TriangulationLOSTExample.cpp @@ -53,24 +53,24 @@ void PrintDuration(const std::chrono::nanoseconds dur, double num_samples, 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); + const double minXY = -10, maxXY = 10; + const double minZ = -20, maxZ = 0; + const int nrCameras = 500; + cameras->reserve(nrCameras); + poses->reserve(nrCameras); + measurements->reserve(nrCameras); *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; + std::uniform_real_distribution rand_xy(minXY, maxXY); + std::uniform_real_distribution rand_z(minZ, maxZ); + Cal3_S2 identityK; - for (int i = 0; i < num_cameras; ++i) { + for (int i = 0; i < nrCameras; ++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); + cameras->emplace_back(wTi, identityK); measurements->push_back(cameras->back().project(*point)); } } @@ -78,84 +78,82 @@ void GetLargeCamerasDataset(CameraSet>* cameras, 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); + Pose3 pose1; + Pose3 pose2(Rot3(), Point3(5., 0., -5.)); + Cal3_S2 identityK; + PinholeCamera camera1(pose1, identityK); + PinholeCamera camera2(pose2, identityK); *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)}; + cameras->push_back(camera1); + cameras->push_back(camera2); + *poses = {pose1, pose2}; + *measurements = {camera1.project(*point), camera2.project(*point)}; } Point2Vector AddNoiseToMeasurements(const Point2Vector& measurements, - const double measurement_sigma) { - std::normal_distribution normal(0.0, measurement_sigma); + const double measurementSigma) { + std::normal_distribution normal(0.0, measurementSigma); - Point2Vector noisy_measurements; - noisy_measurements.reserve(measurements.size()); + Point2Vector noisyMeasurements; + noisyMeasurements.reserve(measurements.size()); for (const auto& p : measurements) { - noisy_measurements.emplace_back(p.x() + normal(rng), p.y() + normal(rng)); + noisyMeasurements.emplace_back(p.x() + normal(rng), p.y() + normal(rng)); } - return noisy_measurements; + return noisyMeasurements; } /* ************************************************************************* */ int main(int argc, char* argv[]) { CameraSet> cameras; std::vector poses; - Point3 gt_point; + Point3 landmark; 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); + GetLargeCamerasDataset(&cameras, &poses, &landmark, &measurements); + // GetSmallCamerasDataset(&cameras, &poses, &landmark, &measurements); + const double measurementSigma = 1e-2; + SharedNoiseModel measurementNoise = + noiseModel::Isotropic::Sigma(2, measurementSigma); - 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); + const long int nrTrials = 1000; + Matrix errorsDLT = Matrix::Zero(nrTrials, 3); + Matrix errorsLOST = Matrix::Zero(nrTrials, 3); + Matrix errorsDLTOpt = Matrix::Zero(nrTrials, 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; + std::chrono::nanoseconds durationDLT; + std::chrono::nanoseconds durationDLTOpt; + std::chrono::nanoseconds durationLOST; - for (int i = 0; i < num_trials; i++) { - Point2Vector noisy_measurements = - AddNoiseToMeasurements(measurements, measurement_sigma); + for (int i = 0; i < nrTrials; i++) { + Point2Vector noisyMeasurements = + AddNoiseToMeasurements(measurements, measurementSigma); - 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 lostStart = std::chrono::high_resolution_clock::now(); + boost::optional estimateLOST = triangulatePoint3( + cameras, noisyMeasurements, rank_tol, false, measurementNoise, true); + durationLOST += std::chrono::high_resolution_clock::now() - lostStart; - 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 dltStart = std::chrono::high_resolution_clock::now(); + boost::optional estimateDLT = triangulatePoint3( + cameras, noisyMeasurements, rank_tol, false, measurementNoise, false); + durationDLT += std::chrono::high_resolution_clock::now() - dltStart; - 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; + auto dltOptStart = std::chrono::high_resolution_clock::now(); + boost::optional estimateDLTOpt = triangulatePoint3( + cameras, noisyMeasurements, rank_tol, true, measurementNoise, false); + durationDLTOpt += std::chrono::high_resolution_clock::now() - dltOptStart; - 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; + errorsLOST.row(i) = *estimateLOST - landmark; + errorsDLT.row(i) = *estimateDLT - landmark; + errorsDLTOpt.row(i) = *estimateDLTOpt - landmark; } - PrintCovarianceStats(lost_errors, "LOST"); - PrintCovarianceStats(dlt_errors, "DLT"); - PrintCovarianceStats(dlt_opt_errors, "DLT_OPT"); + PrintCovarianceStats(errorsLOST, "LOST"); + PrintCovarianceStats(errorsDLT, "DLT"); + PrintCovarianceStats(errorsDLTOpt, "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 + PrintDuration(durationLOST, nrTrials, "LOST"); + PrintDuration(durationDLT, nrTrials, "DLT"); + PrintDuration(durationDLTOpt, nrTrials, "DLT_OPT"); +} diff --git a/gtsam/geometry/triangulation.h b/gtsam/geometry/triangulation.h index aa9433484..f6c919b24 100644 --- a/gtsam/geometry/triangulation.h +++ b/gtsam/geometry/triangulation.h @@ -317,10 +317,10 @@ template typename CAMERA::MeasurementVector undistortMeasurements( const CameraSet& cameras, const typename CAMERA::MeasurementVector& measurements) { - const size_t num_meas = measurements.size(); - assert(num_meas == cameras.size()); - typename CAMERA::MeasurementVector undistortedMeasurements(num_meas); - for (size_t ii = 0; ii < num_meas; ++ii) { + const size_t nrMeasurements = measurements.size(); + assert(nrMeasurements == cameras.size()); + typename CAMERA::MeasurementVector undistortedMeasurements(nrMeasurements); + for (size_t ii = 0; ii < nrMeasurements; ++ii) { // Calibrate with cal and uncalibrate with pinhole version of cal so that // measurements are undistorted. undistortedMeasurements[ii] = @@ -382,10 +382,10 @@ template inline Point3Vector calibrateMeasurements( const CameraSet& cameras, const typename CAMERA::MeasurementVector& measurements) { - const size_t num_meas = measurements.size(); - assert(num_meas == cameras.size()); - Point3Vector calibratedMeasurements(num_meas); - for (size_t ii = 0; ii < num_meas; ++ii) { + const size_t nrMeasurements = measurements.size(); + assert(nrMeasurements == cameras.size()); + Point3Vector calibratedMeasurements(nrMeasurements); + for (size_t ii = 0; ii < nrMeasurements; ++ii) { calibratedMeasurements[ii] << cameras[ii].calibration().calibrate(measurements[ii]), 1.0;