StereoFrame::StereoFrame(
const size_t id,
const CameraPose& cameraPose, const CameraParameters& calibrationLeft,
const double stereo_baseline, const CameraParameters& calibrationRight,
const ImageFeatures& imageFeaturesLeft, const ImageFeatures& imageFeaturesRight,
bool bFixed
)
: frameLeft_(Camera(cameraPose, calibrationLeft), imageFeaturesLeft)
, frameRight_(Camera(ComputeRightCameraPose(cameraPose, stereo_baseline), calibrationRight), imageFeaturesRight)
, rectified_camera_parameters_({stereo_baseline, calibrationLeft.focalLengths(), calibrationLeft.principalPoint()})
, bFixed_( bFixed ), id_( id )
{}
/// Perform the undistortion of LED measurements.
inline std::vector<LedMeasurement>
undistortLeds(std::vector<LedMeasurement> const &distortedMeasurements,
CameraParameters const &camParams) {
std::vector<LedMeasurement> ret;
ret.resize(distortedMeasurements.size());
auto distortionModel = CameraDistortionModel{
Eigen::Vector2d{camParams.focalLengthX(), camParams.focalLengthY()},
cvToVector(camParams.principalPoint()),
Eigen::Vector3d{camParams.k1(), camParams.k2(), camParams.k3()}};
auto ledUndistort = [&distortionModel](LedMeasurement const &meas) {
LedMeasurement ret{meas};
Eigen::Vector2d undistorted = distortionModel.undistortPoint(
cvToVector(meas.loc).cast<double>());
ret.loc = vecToPoint(undistorted.cast<float>());
return ret;
};
std::transform(begin(distortedMeasurements), end(distortedMeasurements),
begin(ret), ledUndistort);
return ret;
}