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; }