forked from pz4kybsvg/Conception
You can not select more than 25 topics
Topics must start with a letter or number, can include dashes ('-') and can be up to 35 characters long.
98 lines
3.3 KiB
98 lines
3.3 KiB
#include "drake/systems/sensors/camera_info.h"
|
|
|
|
#include <cmath>
|
|
#include <sstream>
|
|
#include <string>
|
|
|
|
#include "drake/common/drake_assert.h"
|
|
#include "drake/common/fmt_eigen.h"
|
|
|
|
namespace drake {
|
|
namespace systems {
|
|
namespace sensors {
|
|
namespace {
|
|
|
|
constexpr char kPrefix[] = "\n ";
|
|
|
|
template <typename T>
|
|
void FormatNVP(const char* name, T value, std::stringstream* s) {
|
|
(*s) << kPrefix << name << " (" << value << ")";
|
|
}
|
|
|
|
void CheckPositive(const char* name, int value, std::stringstream* s) {
|
|
if (value <= 0) {
|
|
FormatNVP(name, value, s);
|
|
(*s) << " should be positive.";
|
|
}
|
|
}
|
|
|
|
void CheckBetweenZeroAndMax(const char* name, double value, int max,
|
|
std::stringstream* s) {
|
|
if (value <= 0 || value >= static_cast<double>(max)) {
|
|
FormatNVP(name, value, s);
|
|
(*s) << " should lie in the range (0, " << max << ").";
|
|
}
|
|
}
|
|
|
|
void CheckPositiveFinite(const char* name, double value, std::stringstream* s) {
|
|
if (value <= 0 || !std::isfinite(value)) {
|
|
FormatNVP(name, value, s);
|
|
(*s) << " should be a positive, finite number.";
|
|
}
|
|
}
|
|
|
|
} // namespace
|
|
|
|
CameraInfo::CameraInfo(int width, int height, double focal_x, double focal_y,
|
|
double center_x, double center_y)
|
|
: CameraInfo(width, height, (
|
|
Eigen::Matrix3d() << focal_x, 0., center_x,
|
|
0., focal_y, center_y,
|
|
0., 0., 1.).finished()) {}
|
|
|
|
CameraInfo::CameraInfo(
|
|
int width, int height, const Eigen::Matrix3d& intrinsic_matrix)
|
|
: width_(width), height_(height),
|
|
intrinsic_matrix_(intrinsic_matrix) {
|
|
std::stringstream errors;
|
|
|
|
CheckPositive("Width", width, &errors);
|
|
CheckPositive("Height", height, &errors);
|
|
|
|
const Eigen::Matrix3d& K = intrinsic_matrix;
|
|
CheckPositiveFinite("Focal X", K(0, 0), &errors);
|
|
CheckPositiveFinite("Focal Y", K(1, 1), &errors);
|
|
CheckBetweenZeroAndMax("Center X", K(0, 2), width, &errors);
|
|
CheckBetweenZeroAndMax("Center Y", K(1, 2), height, &errors);
|
|
|
|
// Off-diagonal terms should be zero and homogeneous row should be [0, 0, 1].
|
|
// TODO(eric.cousineau): Relax this with a tolerance?
|
|
if (K(0, 1) != 0 || K(1, 0) != 0 || K(2, 0) != 0 || K(2, 1) != 0 ||
|
|
K(2, 2) != 1) {
|
|
errors << kPrefix << "The camera's intrinsic matrix is malformed:\n"
|
|
<< fmt::to_string(fmt_eigen(K));
|
|
}
|
|
|
|
const std::string error_message = errors.str();
|
|
if (!error_message.empty()) {
|
|
throw std::runtime_error("Invalid camera configuration: " + error_message);
|
|
}
|
|
}
|
|
|
|
CameraInfo::CameraInfo(int width, int height, double vertical_fov_rad)
|
|
: CameraInfo(width, height,
|
|
height * 0.5 / std::tan(0.5 * vertical_fov_rad),
|
|
height * 0.5 / std::tan(0.5 * vertical_fov_rad),
|
|
width * 0.5 - 0.5, height * 0.5 - 0.5) {}
|
|
// TODO(SeanCurtis-TRI): The shift of the principal point by (-0.5, -0.5) is
|
|
// overly opaque. The primary explanation comes from pixel addressing
|
|
// conventions used in various APIs (see
|
|
// https://www.khronos.org/registry/OpenGL/extensions/ARB/ARB_fragment_coord_conventions.txt
|
|
// However, we don't want to look like this class is coupled with OpenGL. How
|
|
// do we articulate this math in a way that *doesn't* depend on OpenGL?
|
|
// See https://github.com/SeanCurtis-TRI/drake/pull/5#pullrequestreview-264447958.
|
|
|
|
} // namespace sensors
|
|
} // namespace systems
|
|
} // namespace drake
|