Exemple #1
0
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;
}