12345qiupeng
/
gtsam
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

more renames to camelcase

release/4.3a0
Akshay Krishnan 2022-07-12 20:03:02 -07:00
parent f347209d4c
commit 897dae4436
2 changed files with 72 additions and 74 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

130
examples/TriangulationLOSTExample.cpp
View File

@ -53,24 +53,24 @@ void PrintDuration(const std::chrono::nanoseconds dur, double num_samples,
void GetLargeCamerasDataset(CameraSet<PinholeCamera<Cal3_S2>>* cameras, void GetLargeCamerasDataset(CameraSet<PinholeCamera<Cal3_S2>>* cameras,
std::vector<Pose3>* poses, Point3* point, std::vector<Pose3>* poses, Point3* point,
Point2Vector* measurements) { Point2Vector* measurements) {
const double min_xy = -10, max_xy = 10; const double minXY = -10, maxXY = 10;
const double min_z = -20, max_z = 0; const double minZ = -20, maxZ = 0;
const int num_cameras = 500; const int nrCameras = 500;
cameras->reserve(num_cameras); cameras->reserve(nrCameras);
poses->reserve(num_cameras); poses->reserve(nrCameras);
measurements->reserve(num_cameras); measurements->reserve(nrCameras);
*point = Point3(0.0, 0.0, 10.0); *point = Point3(0.0, 0.0, 10.0);
std::uniform_real_distribution<double> rand_xy(min_xy, max_xy); std::uniform_real_distribution<double> rand_xy(minXY, maxXY);
std::uniform_real_distribution<double> rand_z(min_z, max_z); std::uniform_real_distribution<double> rand_z(minZ, maxZ);
Cal3_S2 identity_K; 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)); Point3 wti(rand_xy(rng), rand_xy(rng), rand_z(rng));
Pose3 wTi(Rot3(), wti); Pose3 wTi(Rot3(), wti);
poses->push_back(wTi); poses->push_back(wTi);
cameras->emplace_back(wTi, identity_K); cameras->emplace_back(wTi, identityK);
measurements->push_back(cameras->back().project(*point)); measurements->push_back(cameras->back().project(*point));
} }
} }
@ -78,84 +78,82 @@ void GetLargeCamerasDataset(CameraSet<PinholeCamera<Cal3_S2>>* cameras,
void GetSmallCamerasDataset(CameraSet<PinholeCamera<Cal3_S2>>* cameras, void GetSmallCamerasDataset(CameraSet<PinholeCamera<Cal3_S2>>* cameras,
std::vector<Pose3>* poses, Point3* point, std::vector<Pose3>* poses, Point3* point,
Point2Vector* measurements) { Point2Vector* measurements) {
Pose3 pose_1; Pose3 pose1;
Pose3 pose_2(Rot3(), Point3(5., 0., -5.)); Pose3 pose2(Rot3(), Point3(5., 0., -5.));
Cal3_S2 identity_K; Cal3_S2 identityK;
PinholeCamera<Cal3_S2> camera_1(pose_1, identity_K); PinholeCamera<Cal3_S2> camera1(pose1, identityK);
PinholeCamera<Cal3_S2> camera_2(pose_2, identity_K); PinholeCamera<Cal3_S2> camera2(pose2, identityK);
*point = Point3(0, 0, 1); *point = Point3(0, 0, 1);
cameras->push_back(camera_1); cameras->push_back(camera1);
cameras->push_back(camera_2); cameras->push_back(camera2);
*poses = {pose_1, pose_2}; *poses = {pose1, pose2};
*measurements = {camera_1.project(*point), camera_2.project(*point)}; *measurements = {camera1.project(*point), camera2.project(*point)};
} }
Point2Vector AddNoiseToMeasurements(const Point2Vector& measurements, Point2Vector AddNoiseToMeasurements(const Point2Vector& measurements,
const double measurement_sigma) { const double measurementSigma) {
std::normal_distribution<double> normal(0.0, measurement_sigma); std::normal_distribution<double> normal(0.0, measurementSigma);
Point2Vector noisy_measurements; Point2Vector noisyMeasurements;
noisy_measurements.reserve(measurements.size()); noisyMeasurements.reserve(measurements.size());
for (const auto& p : measurements) { 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[]) { int main(int argc, char* argv[]) {
CameraSet<PinholeCamera<Cal3_S2>> cameras; CameraSet<PinholeCamera<Cal3_S2>> cameras;
std::vector<Pose3> poses; std::vector<Pose3> poses;
Point3 gt_point; Point3 landmark;
Point2Vector measurements; Point2Vector measurements;
GetLargeCamerasDataset(&cameras, &poses, &gt_point, &measurements); GetLargeCamerasDataset(&cameras, &poses, &landmark, &measurements);
// GetSmallCamerasDataset(&cameras, &poses, &gt_point, &measurements); // GetSmallCamerasDataset(&cameras, &poses, &landmark, &measurements);
const double measurement_sigma = 1e-2; const double measurementSigma = 1e-2;
SharedNoiseModel measurement_noise = SharedNoiseModel measurementNoise =
noiseModel::Isotropic::Sigma(2, measurement_sigma); noiseModel::Isotropic::Sigma(2, measurementSigma);
const long int num_trials = 1000; const long int nrTrials = 1000;
Matrix dlt_errors = Matrix::Zero(num_trials, 3); Matrix errorsDLT = Matrix::Zero(nrTrials, 3);
Matrix lost_errors = Matrix::Zero(num_trials, 3); Matrix errorsLOST = Matrix::Zero(nrTrials, 3);
Matrix dlt_opt_errors = Matrix::Zero(num_trials, 3); Matrix errorsDLTOpt = Matrix::Zero(nrTrials, 3);
double rank_tol = 1e-9; double rank_tol = 1e-9;
boost::shared_ptr<Cal3_S2> calib = boost::make_shared<Cal3_S2>(); boost::shared_ptr<Cal3_S2> calib = boost::make_shared<Cal3_S2>();
std::chrono::nanoseconds dlt_duration; std::chrono::nanoseconds durationDLT;
std::chrono::nanoseconds dlt_opt_duration; std::chrono::nanoseconds durationDLTOpt;
std::chrono::nanoseconds lost_duration; std::chrono::nanoseconds durationLOST;
std::chrono::nanoseconds lost_tls_duration;
for (int i = 0; i < num_trials; i++) { for (int i = 0; i < nrTrials; i++) {
Point2Vector noisy_measurements = Point2Vector noisyMeasurements =
AddNoiseToMeasurements(measurements, measurement_sigma); AddNoiseToMeasurements(measurements, measurementSigma);
auto lost_start = std::chrono::high_resolution_clock::now(); auto lostStart = std::chrono::high_resolution_clock::now();
boost::optional<Point3> estimate_lost = triangulatePoint3<Cal3_S2>( boost::optional<Point3> estimateLOST = triangulatePoint3<Cal3_S2>(
cameras, noisy_measurements, rank_tol, false, measurement_noise, true); cameras, noisyMeasurements, rank_tol, false, measurementNoise, true);
lost_duration += std::chrono::high_resolution_clock::now() - lost_start; durationLOST += std::chrono::high_resolution_clock::now() - lostStart;
auto dlt_start = std::chrono::high_resolution_clock::now(); auto dltStart = std::chrono::high_resolution_clock::now();
boost::optional<Point3> estimate_dlt = triangulatePoint3<Cal3_S2>( boost::optional<Point3> estimateDLT = triangulatePoint3<Cal3_S2>(
cameras, noisy_measurements, rank_tol, false, measurement_noise, false); cameras, noisyMeasurements, rank_tol, false, measurementNoise, false);
dlt_duration += std::chrono::high_resolution_clock::now() - dlt_start; durationDLT += std::chrono::high_resolution_clock::now() - dltStart;
auto dlt_opt_start = std::chrono::high_resolution_clock::now(); auto dltOptStart = std::chrono::high_resolution_clock::now();
boost::optional<Point3> estimate_dlt_opt = triangulatePoint3<Cal3_S2>( boost::optional<Point3> estimateDLTOpt = triangulatePoint3<Cal3_S2>(
cameras, noisy_measurements, rank_tol, true, measurement_noise, false); cameras, noisyMeasurements, rank_tol, true, measurementNoise, false);
dlt_opt_duration += durationDLTOpt += std::chrono::high_resolution_clock::now() - dltOptStart;
std::chrono::high_resolution_clock::now() - dlt_opt_start;
lost_errors.row(i) = *estimate_lost - gt_point; errorsLOST.row(i) = *estimateLOST - landmark;
dlt_errors.row(i) = *estimate_dlt - gt_point; errorsDLT.row(i) = *estimateDLT - landmark;
dlt_opt_errors.row(i) = *estimate_dlt_opt - gt_point; errorsDLTOpt.row(i) = *estimateDLTOpt - landmark;
} }
PrintCovarianceStats(lost_errors, "LOST"); PrintCovarianceStats(errorsLOST, "LOST");
PrintCovarianceStats(dlt_errors, "DLT"); PrintCovarianceStats(errorsDLT, "DLT");
PrintCovarianceStats(dlt_opt_errors, "DLT_OPT"); PrintCovarianceStats(errorsDLTOpt, "DLT_OPT");
PrintDuration(lost_duration, num_trials, "LOST"); PrintDuration(durationLOST, nrTrials, "LOST");
PrintDuration(dlt_duration, num_trials, "DLT"); PrintDuration(durationDLT, nrTrials, "DLT");
PrintDuration(dlt_opt_duration, num_trials, "DLT_OPT"); PrintDuration(durationDLTOpt, nrTrials, "DLT_OPT");
} }

16
gtsam/geometry/triangulation.h
View File

@ -317,10 +317,10 @@ template <class CAMERA>
typename CAMERA::MeasurementVector undistortMeasurements( typename CAMERA::MeasurementVector undistortMeasurements(
const CameraSet<CAMERA>& cameras, const CameraSet<CAMERA>& cameras,
const typename CAMERA::MeasurementVector& measurements) { const typename CAMERA::MeasurementVector& measurements) {
const size_t num_meas = measurements.size(); const size_t nrMeasurements = measurements.size();
assert(num_meas == cameras.size()); assert(nrMeasurements == cameras.size());
typename CAMERA::MeasurementVector undistortedMeasurements(num_meas); typename CAMERA::MeasurementVector undistortedMeasurements(nrMeasurements);
for (size_t ii = 0; ii < num_meas; ++ii) { for (size_t ii = 0; ii < nrMeasurements; ++ii) {
// Calibrate with cal and uncalibrate with pinhole version of cal so that // Calibrate with cal and uncalibrate with pinhole version of cal so that
// measurements are undistorted. // measurements are undistorted.
undistortedMeasurements[ii] = undistortedMeasurements[ii] =
@ -382,10 +382,10 @@ template <class CAMERA>
inline Point3Vector calibrateMeasurements( inline Point3Vector calibrateMeasurements(
const CameraSet<CAMERA>& cameras, const CameraSet<CAMERA>& cameras,
const typename CAMERA::MeasurementVector& measurements) { const typename CAMERA::MeasurementVector& measurements) {
const size_t num_meas = measurements.size(); const size_t nrMeasurements = measurements.size();
assert(num_meas == cameras.size()); assert(nrMeasurements == cameras.size());
Point3Vector calibratedMeasurements(num_meas); Point3Vector calibratedMeasurements(nrMeasurements);
for (size_t ii = 0; ii < num_meas; ++ii) { for (size_t ii = 0; ii < nrMeasurements; ++ii) {
calibratedMeasurements[ii] calibratedMeasurements[ii]
<< cameras[ii].calibration().calibrate(measurements[ii]), << cameras[ii].calibration().calibrate(measurements[ii]),
1.0; 1.0;