more renames to camelcase
Akshay Krishnan
parent
f347209d4c
commit
897dae4436
|
|
@ -53,24 +53,24 @@ void PrintDuration(const std::chrono::nanoseconds dur, double num_samples,
|
|||
void GetLargeCamerasDataset(CameraSet<PinholeCamera<Cal3_S2>>* cameras,
|
||||
std::vector<Pose3>* 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<double> rand_xy(min_xy, max_xy);
|
||||
std::uniform_real_distribution<double> rand_z(min_z, max_z);
|
||||
Cal3_S2 identity_K;
|
||||
std::uniform_real_distribution<double> rand_xy(minXY, maxXY);
|
||||
std::uniform_real_distribution<double> 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<PinholeCamera<Cal3_S2>>* cameras,
|
|||
void GetSmallCamerasDataset(CameraSet<PinholeCamera<Cal3_S2>>* cameras,
|
||||
std::vector<Pose3>* poses, Point3* point,
|
||||
Point2Vector* measurements) {
|
||||
Pose3 pose_1;
|
||||
Pose3 pose_2(Rot3(), Point3(5., 0., -5.));
|
||||
Cal3_S2 identity_K;
|
||||
PinholeCamera<Cal3_S2> camera_1(pose_1, identity_K);
|
||||
PinholeCamera<Cal3_S2> camera_2(pose_2, identity_K);
|
||||
Pose3 pose1;
|
||||
Pose3 pose2(Rot3(), Point3(5., 0., -5.));
|
||||
Cal3_S2 identityK;
|
||||
PinholeCamera<Cal3_S2> camera1(pose1, identityK);
|
||||
PinholeCamera<Cal3_S2> 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<double> normal(0.0, measurement_sigma);
|
||||
const double measurementSigma) {
|
||||
std::normal_distribution<double> 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<PinholeCamera<Cal3_S2>> cameras;
|
||||
std::vector<Pose3> 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<Cal3_S2> calib = boost::make_shared<Cal3_S2>();
|
||||
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<Point3> estimate_lost = triangulatePoint3<Cal3_S2>(
|
||||
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<Point3> estimateLOST = triangulatePoint3<Cal3_S2>(
|
||||
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<Point3> estimate_dlt = triangulatePoint3<Cal3_S2>(
|
||||
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<Point3> estimateDLT = triangulatePoint3<Cal3_S2>(
|
||||
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<Point3> estimate_dlt_opt = triangulatePoint3<Cal3_S2>(
|
||||
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<Point3> estimateDLTOpt = triangulatePoint3<Cal3_S2>(
|
||||
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");
|
||||
}
|
||||
PrintDuration(durationLOST, nrTrials, "LOST");
|
||||
PrintDuration(durationDLT, nrTrials, "DLT");
|
||||
PrintDuration(durationDLTOpt, nrTrials, "DLT_OPT");
|
||||
}
|
||||
|
|
|
|||
|
|
@ -317,10 +317,10 @@ template <class CAMERA>
|
|||
typename CAMERA::MeasurementVector undistortMeasurements(
|
||||
const CameraSet<CAMERA>& 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 <class CAMERA>
|
|||
inline Point3Vector calibrateMeasurements(
|
||||
const CameraSet<CAMERA>& 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;
|
||||
|
|
|
|||
Loading…
Reference in New Issue