Skip to content

Commit

Permalink
Remove ceres/internal/random.h in favor of <random>
Browse files Browse the repository at this point in the history
Fixes #854

Change-Id: Id30b8dc2221f9afe4eb83f3a9304b9b2bc7e05d4
  • Loading branch information
sandwichmaker committed Aug 8, 2022
1 parent d881b5c commit 5d0bca1
Show file tree
Hide file tree
Showing 19 changed files with 153 additions and 196 deletions.
2 changes: 0 additions & 2 deletions examples/BUILD
Original file line number Diff line number Diff line change
Expand Up @@ -47,7 +47,6 @@ cc_binary(
"bal_problem.cc",
"bal_problem.h",
"bundle_adjuster.cc",
"random.h",
"snavely_reprojection_error.h",
],
copts = EXAMPLE_COPTS,
Expand All @@ -69,7 +68,6 @@ cc_binary(
cc_binary(
name = "robot_pose_mle",
srcs = [
"random.h",
"robot_pose_mle.cc",
],
copts = EXAMPLE_COPTS,
Expand Down
23 changes: 15 additions & 8 deletions examples/bal_problem.cc
Original file line number Diff line number Diff line change
Expand Up @@ -32,15 +32,15 @@

#include <algorithm>
#include <cstdio>
#include <cstdlib>
#include <fstream>
#include <functional>
#include <random>
#include <string>
#include <vector>

#include "Eigen/Core"
#include "ceres/rotation.h"
#include "glog/logging.h"
#include "random.h"

namespace ceres::examples {
namespace {
Expand All @@ -55,9 +55,9 @@ void FscanfOrDie(FILE* fptr, const char* format, T* value) {
}
}

void PerturbPoint3(const double sigma, double* point) {
void PerturbPoint3(std::function<double()> dist, double* point) {
for (int i = 0; i < 3; ++i) {
point[i] += RandNormal() * sigma;
point[i] += dist();
}
}

Expand Down Expand Up @@ -297,14 +297,19 @@ void BALProblem::Perturb(const double rotation_sigma,
CHECK_GE(point_sigma, 0.0);
CHECK_GE(rotation_sigma, 0.0);
CHECK_GE(translation_sigma, 0.0);

std::mt19937 prng;
std::normal_distribution point_noise_distribution(0.0, point_sigma);
double* points = mutable_points();
if (point_sigma > 0) {
for (int i = 0; i < num_points_; ++i) {
PerturbPoint3(point_sigma, points + 3 * i);
PerturbPoint3(std::bind(point_noise_distribution, std::ref(prng)),
points + 3 * i);
}
}

std::normal_distribution rotation_noise_distribution(0.0, point_sigma);
std::normal_distribution translation_noise_distribution(0.0,
translation_sigma);
for (int i = 0; i < num_cameras_; ++i) {
double* camera = mutable_cameras() + camera_block_size() * i;

Expand All @@ -314,12 +319,14 @@ void BALProblem::Perturb(const double rotation_sigma,
// representation.
CameraToAngleAxisAndCenter(camera, angle_axis, center);
if (rotation_sigma > 0.0) {
PerturbPoint3(rotation_sigma, angle_axis);
PerturbPoint3(std::bind(rotation_noise_distribution, std::ref(prng)),
angle_axis);
}
AngleAxisAndCenterToCamera(angle_axis, center, camera);

if (translation_sigma > 0.0) {
PerturbPoint3(translation_sigma, camera + camera_block_size() - 6);
PerturbPoint3(std::bind(translation_noise_distribution, std::ref(prng)),
camera + camera_block_size() - 6);
}
}
}
Expand Down
13 changes: 7 additions & 6 deletions examples/robot_pose_mle.cc
Original file line number Diff line number Diff line change
Expand Up @@ -127,13 +127,13 @@

#include <cmath>
#include <cstdio>
#include <random>
#include <vector>

#include "ceres/ceres.h"
#include "ceres/dynamic_autodiff_cost_function.h"
#include "gflags/gflags.h"
#include "glog/logging.h"
#include "random.h"

using ceres::AutoDiffCostFunction;
using ceres::CauchyLoss;
Expand All @@ -143,7 +143,6 @@ using ceres::LossFunction;
using ceres::Problem;
using ceres::Solve;
using ceres::Solver;
using ceres::examples::RandNormal;
using std::min;
using std::vector;

Expand Down Expand Up @@ -247,6 +246,10 @@ void SimulateRobot(vector<double>* odometry_values,
const int num_steps =
static_cast<int>(ceil(CERES_GET_FLAG(FLAGS_corridor_length) /
CERES_GET_FLAG(FLAGS_pose_separation)));
std::mt19937 prng;
std::normal_distribution odometry_noise(
0.0, CERES_GET_FLAG(FLAGS_odometry_stddev));
std::normal_distribution range_noise(0.0, CERES_GET_FLAG(FLAGS_range_stddev));

// The robot starts out at the origin.
double robot_location = 0.0;
Expand All @@ -258,10 +261,8 @@ void SimulateRobot(vector<double>* odometry_values,
const double actual_range =
CERES_GET_FLAG(FLAGS_corridor_length) - robot_location;
const double observed_odometry =
RandNormal() * CERES_GET_FLAG(FLAGS_odometry_stddev) +
actual_odometry_value;
const double observed_range =
RandNormal() * CERES_GET_FLAG(FLAGS_range_stddev) + actual_range;
actual_odometry_value + odometry_noise(prng);
const double observed_range = actual_range + range_noise(prng);
odometry_values->push_back(observed_odometry);
range_readings->push_back(observed_range);
}
Expand Down
15 changes: 9 additions & 6 deletions internal/ceres/autodiff_test.cc
Original file line number Diff line number Diff line change
Expand Up @@ -30,7 +30,8 @@

#include "ceres/internal/autodiff.h"

#include "ceres/random.h"
#include <random>

#include "gtest/gtest.h"

namespace ceres::internal {
Expand Down Expand Up @@ -162,17 +163,18 @@ struct Projective {

// Test projective camera model projector.
TEST(AutoDiff, ProjectiveCameraModel) {
srand(5);
double const tol = 1e-10; // floating-point tolerance.
double const del = 1e-4; // finite-difference step.
double const err = 1e-6; // finite-difference tolerance.

Projective b;
std::mt19937 prng;
std::uniform_real_distribution<double> uniform01(0.0, 1.0);

// Make random P and X, in a single vector.
double PX[12 + 4];
for (double& PX_i : PX) {
PX_i = RandDouble();
PX_i = uniform01(prng);
}

// Handy names for the P and X parts.
Expand Down Expand Up @@ -282,16 +284,17 @@ struct Metric {

// This test is similar in structure to the previous one.
TEST(AutoDiff, Metric) {
srand(5);
double const tol = 1e-10; // floating-point tolerance.
double const del = 1e-4; // finite-difference step.
double const err = 1e-5; // finite-difference tolerance.
double const err = 2e-5; // finite-difference tolerance.

Metric b;

// Make random parameter vector.
double qcX[4 + 3 + 3];
for (double& qcX_i : qcX) qcX_i = RandDouble();
std::mt19937 prng;
std::uniform_real_distribution<double> uniform01(0.0, 1.0);
for (double& qcX_i : qcX) qcX_i = uniform01(prng);

// Handy names.
double* q = qcX;
Expand Down
22 changes: 12 additions & 10 deletions internal/ceres/block_sparse_matrix.cc
Original file line number Diff line number Diff line change
Expand Up @@ -33,12 +33,12 @@
#include <algorithm>
#include <cstddef>
#include <memory>
#include <random>
#include <vector>

#include "ceres/block_structure.h"
#include "ceres/crs_matrix.h"
#include "ceres/internal/eigen.h"
#include "ceres/random.h"
#include "ceres/small_blas.h"
#include "ceres/triplet_sparse_matrix.h"
#include "glog/logging.h"
Expand Down Expand Up @@ -382,6 +382,11 @@ std::unique_ptr<BlockSparseMatrix> BlockSparseMatrix::CreateRandomMatrix(
CHECK_GT(options.block_density, 0.0);
CHECK_LE(options.block_density, 1.0);

std::mt19937 prng;
std::uniform_int_distribution col_distribution(options.min_col_block_size,
options.max_col_block_size);
std::uniform_int_distribution row_distribution(options.min_row_block_size,
options.max_row_block_size);
auto* bs = new CompressedRowBlockStructure();
if (options.col_blocks.empty()) {
CHECK_GT(options.num_col_blocks, 0);
Expand All @@ -392,10 +397,7 @@ std::unique_ptr<BlockSparseMatrix> BlockSparseMatrix::CreateRandomMatrix(
// Generate the col block structure.
int col_block_position = 0;
for (int i = 0; i < options.num_col_blocks; ++i) {
// Generate a random integer in [min_col_block_size, max_col_block_size]
const int delta_block_size =
Uniform(options.max_col_block_size - options.min_col_block_size);
const int col_block_size = options.min_col_block_size + delta_block_size;
const int col_block_size = col_distribution(prng);
bs->cols.emplace_back(col_block_size, col_block_position);
col_block_position += col_block_size;
}
Expand All @@ -404,22 +406,21 @@ std::unique_ptr<BlockSparseMatrix> BlockSparseMatrix::CreateRandomMatrix(
}

bool matrix_has_blocks = false;
std::uniform_real_distribution uniform01(0.0, 1.0);
while (!matrix_has_blocks) {
VLOG(1) << "Clearing";
bs->rows.clear();
int row_block_position = 0;
int value_position = 0;
for (int r = 0; r < options.num_row_blocks; ++r) {
const int delta_block_size =
Uniform(options.max_row_block_size - options.min_row_block_size);
const int row_block_size = options.min_row_block_size + delta_block_size;
const int row_block_size = row_distribution(prng);
bs->rows.emplace_back();
CompressedRow& row = bs->rows.back();
row.block.size = row_block_size;
row.block.position = row_block_position;
row_block_position += row_block_size;
for (int c = 0; c < bs->cols.size(); ++c) {
if (RandDouble() > options.block_density) continue;
if (uniform01(prng) > options.block_density) continue;

row.cells.emplace_back();
Cell& cell = row.cells.back();
Expand All @@ -433,8 +434,9 @@ std::unique_ptr<BlockSparseMatrix> BlockSparseMatrix::CreateRandomMatrix(

auto matrix = std::make_unique<BlockSparseMatrix>(bs);
double* values = matrix->mutable_values();
std::normal_distribution standard_normal_distribution;
for (int i = 0; i < matrix->num_nonzeros(); ++i) {
values[i] = RandNormal();
values[i] = standard_normal_distribution(prng);
}

return matrix;
Expand Down
30 changes: 20 additions & 10 deletions internal/ceres/compressed_row_sparse_matrix.cc
Original file line number Diff line number Diff line change
Expand Up @@ -31,13 +31,14 @@
#include "ceres/compressed_row_sparse_matrix.h"

#include <algorithm>
#include <functional>
#include <memory>
#include <numeric>
#include <random>
#include <vector>

#include "ceres/crs_matrix.h"
#include "ceres/internal/export.h"
#include "ceres/random.h"
#include "ceres/triplet_sparse_matrix.h"
#include "glog/logging.h"

Expand Down Expand Up @@ -122,26 +123,28 @@ void AddRandomBlock(const int num_rows,
const int num_cols,
const int row_block_begin,
const int col_block_begin,
std::function<double()> dist,
std::vector<int>* rows,
std::vector<int>* cols,
std::vector<double>* values) {
for (int r = 0; r < num_rows; ++r) {
for (int c = 0; c < num_cols; ++c) {
rows->push_back(row_block_begin + r);
cols->push_back(col_block_begin + c);
values->push_back(RandNormal());
values->push_back(dist());
}
}
}

void AddSymmetricRandomBlock(const int num_rows,
const int row_block_begin,
std::function<double()> dist,
std::vector<int>* rows,
std::vector<int>* cols,
std::vector<double>* values) {
for (int r = 0; r < num_rows; ++r) {
for (int c = r; c < num_rows; ++c) {
const double v = RandNormal();
const double v = dist();
rows->push_back(row_block_begin + r);
cols->push_back(row_block_begin + c);
values->push_back(v);
Expand Down Expand Up @@ -644,21 +647,26 @@ CompressedRowSparseMatrix::CreateRandomMatrix(
vector<int> row_blocks;
vector<int> col_blocks;

std::mt19937 prng;
std::uniform_int_distribution col_distribution(options.min_col_block_size,
options.max_col_block_size);
std::uniform_int_distribution row_distribution(options.min_row_block_size,
options.max_row_block_size);
std::uniform_real_distribution uniform01(0.0, 1.0);
std::normal_distribution standard_normal_distribution;
auto values_dist = std::bind(standard_normal_distribution, std::ref(prng));

// Generate the row block structure.
for (int i = 0; i < options.num_row_blocks; ++i) {
// Generate a random integer in [min_row_block_size, max_row_block_size]
const int delta_block_size =
Uniform(options.max_row_block_size - options.min_row_block_size);
row_blocks.push_back(options.min_row_block_size + delta_block_size);
row_blocks.push_back(row_distribution(prng));
}

if (options.storage_type == StorageType::UNSYMMETRIC) {
// Generate the col block structure.
for (int i = 0; i < options.num_col_blocks; ++i) {
// Generate a random integer in [min_col_block_size, max_col_block_size]
const int delta_block_size =
Uniform(options.max_col_block_size - options.min_col_block_size);
col_blocks.push_back(options.min_col_block_size + delta_block_size);
col_blocks.push_back(col_distribution(prng));
}
} else {
// Symmetric matrices (LOWER_TRIANGULAR or UPPER_TRIANGULAR);
Expand Down Expand Up @@ -695,20 +703,22 @@ CompressedRowSparseMatrix::CreateRandomMatrix(
}

// Randomly determine if this block is present or not.
if (RandDouble() <= options.block_density) {
if (uniform01(prng) <= options.block_density) {
// If the matrix is symmetric, then we take care to generate
// symmetric diagonal blocks.
if (options.storage_type == StorageType::UNSYMMETRIC || r != c) {
AddRandomBlock(row_blocks[r],
col_blocks[c],
row_block_begin,
col_block_begin,
values_dist,
&tsm_rows,
&tsm_cols,
&tsm_values);
} else {
AddSymmetricRandomBlock(row_blocks[r],
row_block_begin,
values_dist,
&tsm_rows,
&tsm_cols,
&tsm_values);
Expand Down

0 comments on commit 5d0bca1

Please sign in to comment.