namespace sophus¶
Overview¶
Image MutImage, owning images types. More…
namespace sophus { // namespaces namespace sophus::detail; namespace sophus::jet_helpers; // typedefs using RegionI = Region<int> ; using RegionF32 = Region<float> ; using RegionF64 = Region<double> ; using Region2 = Region<Eigen::Array<TScalar, 2, 1>> ; using Region2I = Region2<int> ; using Region2F32 = Region2<float> ; using Region2F64 = Region2<double> ; using Region3 = Region<Eigen::Array<TScalar, 3, 1>> ; using Region3I = Region3<int> ; using Region3F32 = Region3<float> ; using Region3F64 = Region3<double> ; using Region4 = Region<Eigen::Array<TScalar, 4, 1>> ; using Region4I = Region4<int> ; using Region4F32 = Region4<float> ; using Region4F64 = Region4<double> ; using IsSpecialized = decltype(complete(std::declval<TScalar*>())) ; using IsMappable = IsSpecialized<Eigen::internal::traits<std::decay_t<TScalar>>> ; using InverseDepthPoint3F64 = InverseDepthPoint3<double> ; using Ray2 = Ray<TScalar, 2> ; using Ray3 = Ray<TScalar, 3> ; using Ray2F64 = Ray2<double> ; using Ray3F64 = Ray3<double> ; using UnitVector3 = UnitVector<TScalar, 3> ; using UnitVector2 = UnitVector<TScalar, 2> ; using UnitVector2F64 = UnitVector2<double> ; using UnitVector3F64 = UnitVector3<double> ; using Pixel3 = Eigen::Matrix<TChannel, 3, 1> ; using Pixel3U8 = Pixel3<uint8_t> ; using Pixel3U16 = Pixel3<uint16_t> ; using Pixel3F32 = Pixel3<float> ; using Pixel4 = Eigen::Matrix<TChannel, 4, 1> ; using Pixel4U8 = Pixel4<uint8_t> ; using Pixel4U16 = Pixel4<uint16_t> ; using Pixel4F32 = Pixel4<float> ; using ImageViewBool = ImageView<bool> ; using MutImageViewBool = MutImageView<bool> ; using ImageViewU8 = ImageView<uint8_t> ; using ImageViewU16 = ImageView<uint16_t> ; using ImageViewF32 = ImageView<float> ; using MutImageViewU8 = MutImageView<uint8_t> ; using MutImageViewU16 = MutImageView<uint16_t> ; using MutImageViewF32 = MutImageView<float> ; using ImageView3 = ImageView<Pixel3<TChannel>> ; using ImageView3U8 = ImageView3<uint8_t> ; using ImageView3U16 = ImageView3<uint16_t> ; using ImageView3F32 = ImageView3<float> ; using MutImageView3 = MutImageView<Pixel3<TChannel>> ; using MutImageView3U8 = MutImageView3<uint8_t> ; using MutImageView3U16 = MutImageView3<uint16_t> ; using MutImageView3F32 = MutImageView3<float> ; using ImageView4 = ImageView<Pixel4<TChannel>> ; using ImageView4U8 = ImageView4<uint8_t> ; using ImageView4U16 = ImageView4<uint16_t> ; using ImageView4F32 = ImageView4<float> ; using MutImageView4 = MutImageView<Pixel4<TChannel>> ; using MutImageView4U8 = MutImageView4<uint8_t> ; using MutImageView4U16 = MutImageView4<uint16_t> ; using MutImageView4F32 = MutImageView4<float> ; using ImageBool = Image<bool> ; using MutImageBool = MutImage<bool> ; using ImageU8 = Image<uint8_t> ; using ImageU16 = Image<uint16_t> ; using ImageF32 = Image<float> ; using MutImageU8 = MutImage<uint8_t> ; using MutImageU16 = MutImage<uint16_t> ; using MutImageF32 = MutImage<float> ; using Image3 = Image<Pixel3<TChannel>> ; using Image3U8 = Image3<uint8_t> ; using Image3U16 = Image3<uint16_t> ; using Image3F32 = Image3<float> ; using MutImage3 = MutImage<Pixel3<TChannel>> ; using MutImage3U8 = MutImage3<uint8_t> ; using MutImage3U16 = MutImage3<uint16_t> ; using MutImage3F32 = MutImage3<float> ; using Image4 = Image<Pixel4<TChannel>> ; using Image4U8 = Image4<uint8_t> ; using Image4U16 = Image4<uint16_t> ; using Image4F32 = Image4<float> ; using MutImage4 = MutImage<Pixel4<TChannel>> ; using MutImage4U8 = MutImage4<uint8_t> ; using MutImage4U16 = MutImage4<uint16_t> ; using MutImage4F32 = MutImage4<float> ; using AnyImage = RuntimeImage<AnyImagePredicate, TAllocator> ; using AnyImageView = RuntimeImageView<AnyImagePredicate> ; using IntensityImagePredicate = VariantImagePredicate<std::variant<uint8_t, uint16_t, float, Pixel3U8, Pixel3U16, Pixel3F32, Pixel4U8, Pixel4U16, Pixel4F32>> ; using IntensityImage = RuntimeImage<IntensityImagePredicate, TAllocator> ; using IntensityImageView = RuntimeImageView<IntensityImagePredicate> ; using Cartesian2 = Cartesian<TScalar, 2> ; using Cartesian3 = Cartesian<TScalar, 3> ; using Cartesian2F64 = Cartesian2<double> ; using Cartesian3F64 = Cartesian3<double> ; using Cartesian2d = Cartesian2F64 ; using Cartesian3d = Cartesian3F64 ; using RxSo2F64 = RxSo2<double> ; using RxSo2F32 = RxSo2<float> ; using RxSO2 = RxSo2<TScalar, kOptions> ; using RxSO2d = RxSo2F64 ; using RxSO2f = RxSo2F32 ; using RxSo3F64 = RxSo3<double> ; using RxSo3F32 = RxSo3<float> ; using RxSO3 = RxSo3<TScalar, kOptions> ; using RxSO3d = RxSo3F64 ; using RxSO3f = RxSo3F32 ; using Se2F64 = Se2<double> ; using Se2F32 = Se2<float> ; using SE2 = Se2<TScalar, kOptions> ; using SE2d = Se2F64 ; using SE2f = Se2F32 ; using Se3F64 = Se3<double> ; using Se3F32 = Se3<float> ; using SE3 = Se3<TScalar, kOptions> ; using SE3d = Se3F64 ; using SE3f = Se3F32 ; using Sim2F64 = Sim2<double> ; using Sim2F32 = Sim2<float> ; using Sim2d = Sim2F64 ; using Sim2f = Sim2F32 ; using Sim3F64 = Sim3<double> ; using Sim3F32 = Sim3<float> ; using Sim3d = Sim3F64 ; using Sim3f = Sim3F32 ; using So2F64 = So2<double> ; using So2F32 = So2<float> ; using SO2 = So2<TScalar, kOptions> ; using SO2d = So2F64 ; using SO2f = So2F32 ; using So3F64 = So3<double> ; using So3F32 = So3<float> ; using SO3 = So3<TScalar, kOptions> ; using SO3d = So3F64 ; using SO3f = So3F32 ; using PinholeModel = CameraModelT<double, AffineTransform, ProjectionZ1> ; using BrownConradyModel = CameraModelT<double, BrownConradyTransform, ProjectionZ1> ; using KannalaBrandtK3Model = CameraModelT<double, KannalaBrandtK3Transform, ProjectionZ1> ; using OrthographicModel = CameraModelT<double, AffineTransform, ProjectionOrtho> ; using CameraDistortionVariant = std::variant<PinholeModel, BrownConradyModel, KannalaBrandtK3Model, OrthographicModel> ; using GyroModelVariant = std::variant<ScalingNonOrthogonalityGyroModel<double>> ; using AcceleroModelVariant = std::variant<ScalingNonOrthogonalityAcceleroModel<double>> ; using OrthographicModelT = CameraModelT<TScalar, AffineTransform, ProjectionOrtho> ; // enums enum SegmentCase; // structs struct AnyImagePredicate; template <class TLieGroup> struct BasisSplineSegment; struct CameraInRig; struct ClippingPlanes; template <class TT> struct ClosestApproachResult; template <class TScalar> struct GetScalar; template <class TScalar, int kM, int kMatrixDim> struct GetScalar<Eigen::Matrix<TScalar, kM, kMatrixDim>>; struct ImageSize; template <class TT> struct ImageTraits; template <class TT, int kNumChannelsT> struct ImageTraits<Eigen::Matrix<TT, kNumChannelsT, 1>>; template <class TPixel> struct ImageView; struct IndexAndU; template < typename TVectorT, int kNumDimensions, typename = typename std::enable_if<TVectorT::RowsAtCompileTime == kNumDimensions&& TVectorT::ColsAtCompileTime == 1>::type > struct IsFixedSizeVector; template <class TScalar> struct IsFloatingPoint; template <class TScalar, int kM, int kMatrixDim> struct IsFloatingPoint<Eigen::Matrix<TScalar, kM, kMatrixDim>>; template <class TGenerator> struct IsUniformRandomBitGenerator; template <template<class, int=0> class TLieGroup> struct LieGroupCeresTests; template <class TScalar, typename TE = void> struct Mapper; template <class TScalar> struct Mapper<TScalar, typename std::enable_if<kIsMappableV<TScalar>>::type>; struct MultiCameraRig; template <class TPoint> struct PointTraits; template <ScalarType TPoint> struct PointTraits<TPoint>; struct ProjectionOrtho; struct ProjectionZ1; template <class TScalar> struct Random; template <> struct Random<double>; struct RuntimePixelType; template <class TT> struct ScalingNonOrthogonalityAcceleroModel; template <class TT> struct ScalingNonOrthogonalityGyroModel; struct UninitTag; template <class TPixelVariant> struct VariantImagePredicate; // classes class AffineTransform; template <class TLieGroup> class BasisSpline; template <class TLieGroup> class BasisSplineFn; template <class TLieGroup> class BasisSplineImpl; class BrownConradyTransform; class CameraModel; template <class TScalar, class TDistortion, class TProj = ProjectionZ1> class CameraModelT; template <class TScalar, int kM, int kOptions> class Cartesian; template <class TDerived, int kM> class CartesianBase; template < class TPixel, template<class> class TAllocator = Eigen::aligned_allocator > class Image; class ImageShape; class ImuModel; template <class TT> class InverseDepthPoint3; class KannalaBrandtK3Transform; template <class TLieGroup> class LieGroupTests; template <template<class, int=0> class TLieGroup> class Manifold; template < class TPixel, template<class> class TAllocator = Eigen::aligned_allocator > class MutImage; template <class TPixel> class MutImageView; template <class TT> class PointTransformer; template <class TScalar, int kN> class Ray; template <PointType TPoint> class Region; template < class TPredicate = AnyImagePredicate, template<class> class TAllocator = Eigen::aligned_allocator > class RuntimeImage; template <class TPredicate = AnyImagePredicate> class RuntimeImageView; template <class TScalar, int kOptions> class RxSo2; template <class TDerived> class RxSo2Base; template <class TScalar, int kOptions> class RxSo3; template <class TDerived> class RxSo3Base; template <class TScalar, int kOptions> class Se2; template <class TDerived> class Se2Base; template <class TScalar, int kOptions> class Se3; template <class TDerived> class Se3Base; template <class TScalar, int kOptions> class Sim2; template <class TDerived> class Sim2Base; template <class TScalar, int kOptions> class Sim3; template <class TDerived> class Sim3Base; template <class TScalar, int kOptions> class So2; template <class TDerived> class So2Base; template <class TScalar, int kOptions> class So3; template <class TDerived> class So3Base; template <class TScalar> class SplineBasisFunction; template <class TScalar> class Tests; template <class TScalar, int kN> class UnitVector; // global variables constexpr bool kIsMappableV = IsMappable<TScalar>::value; const TScalar kEpsilon = TScalar(1e-10); const float kEpsilon< float > = float(1e-5); const float kEpsilonF32 = kEpsilon<float>; const float kEpsilonF64 = kEpsilon<double>; const TScalar kEpsilonPlus = kEpsilon<TScalar>*(TScalar(1.) + kEpsilon<TScalar>); const TScalar kEpsilonSqrt = sqrt(kEpsilon<TScalar>); const float kEpsilonSqrtF32 = kEpsilonSqrt<float>; const float kEpsilonSqrtF64 = kEpsilonSqrt<double>; const TScalar kPi = TScalar(3.141592653589793238462643383279502884); const float kPiF32 = kPi<float>; const double kPiF64 = kPi<double>; constexpr bool kIsUniformRandomBitGeneratorV = IsUniformRandomBitGenerator<TGenerator>::kValue; concept DerivedFrom = std::is_base_of_v<TBase, TDerived>; concept SameAs = std::is_same_v<TT, TU>; concept EnumType = std::is_enum_v<TT>; concept Arithmetic = std::is_arithmetic_v<TT>; concept EigenType = DerivedFrom<TDerived, Eigen::EigenBase<TDerived>>; concept EigenDenseType = DerivedFrom<TDerived, Eigen::DenseBase<TDerived>>; concept EigenMatrixType = DerivedFrom<TDerived, Eigen::MatrixBase<TDerived>>; concept EigenArrayType = DerivedFrom<TDerived, Eigen::ArrayBase<TDerived>>; concept EigenSameDim = requires(TT1, TT2) { EigenDenseType<TT1>&& EigenDenseType<TT2>&&(TT1::RowsAtCompileTime == Eigen::Dynamic || TT1::RowsAtCompileTime == TT2::RowsAtCompileTime)&&(TT1::ColsAtCompileTime == Eigen::Dynamic || TT1::ColsAtCompileTime == TT2::ColsAtCompileTime); }; concept EigenWithDim = requires(TT) { EigenDenseType<TT>&& TT::RowsAtCompileTime == kRows&& TT::ColsAtCompileTime == kCols; }; concept EigenWithDimOrDynamic = requires(TT) { EigenDenseType<TT>&&(TT::RowsAtCompileTime == Eigen::Dynamic || TT::RowsAtCompileTime == kRows)&&(TT::ColsAtCompileTime == Eigen::Dynamic || TT::ColsAtCompileTime == kCols); }; concept RealScalarType = std::is_floating_point_v<TT>; concept IntegerScalarType = std::is_integral_v<TT>; concept ScalarType = RealScalarType<TT> || IntegerScalarType<TT>; concept RealEigenDenseType = EigenDenseType<TT>&& std::is_floating_point_v<typename TT::Scalar>; concept IntegerEigenDenseType = EigenDenseType<TT>&& std::is_integral_v<typename TT::Scalar>; concept RealPointType = RealScalarType<TT> || RealEigenDenseType<TT>; concept IntegerPointType = IntegerScalarType<TT> || IntegerEigenDenseType<TT>; concept PointType = RealPointType<TT> || IntegerPointType<TT>; // global functions template <class TScalar, class TFn> auto curveNumDiff( TFn curve, TScalar t, TScalar h = kEpsilonSqrt<TScalar> ); template <class TScalar, int kMatrixDim, int kM, class TScalarOrVector, class TFn> Eigen::Matrix<TScalar, kMatrixDim, kM> vectorFieldNumDiff( TFn vector_field, TScalarOrVector const& a, TScalar eps = kEpsilonSqrt<TScalar> ); template <class TT> bool operator==( Region<TT> const& lhs, Region<TT> const& rhs ); template <int kMatrixDim> double dot( Eigen::Vector<double, kMatrixDim> const& v1, Eigen::Vector<double, kMatrixDim> const& v2 ); double dot( double const& a, double const& b ); template <int kMatrixDim> double squaredNorm(Eigen::Vector<double, kMatrixDim> const& vec); double squaredNorm(double const& scalar); template <class TScalar> TScalar zero(); double zero< double >(); template <class TScalar> TScalar::Scalar* data(TScalar& t); double* data(double& d); template <class TScalar> const TScalar::Scalar* data(TScalar const& t); double const* data(double const& d); template <class TScalar, std::size_t = sizeof(TScalar)> constexpr std::true_type complete(TScalar*); constexpr std::false_type complete(...); template <class TTo, class TT> auto cast(const TT& p); template <ScalarType TPoint> TPoint zero(); template <ScalarType TPoint> auto eval(TPoint const& p); template <ScalarType TPoint> bool allTrue(TPoint const& p); template <ScalarType TPoint> bool anyTrue(TPoint const& p); template <ScalarType TPoint> bool isFinite(TPoint const& p); template <ScalarType TPoint> bool isNan(TPoint const& p); template <ScalarType TPoint> auto square(TPoint const& v); template <ScalarType TPoint> TPoint min( TPoint const& a, TPoint const& b ); template <ScalarType TPoint> TPoint max( TPoint const& a, TPoint const& b ); template <PointType TPoint> TPoint clamp( TPoint const& val, TPoint const& a, TPoint const& b ); template <ScalarType TPoint> auto floor(TPoint s); template <ScalarType TPoint> auto ceil(TPoint s); template <ScalarType TPoint> auto round(TPoint s); template <ScalarType TPoint> auto plus( TPoint p, TPoint s ); template <EigenDenseType TPoint> auto plus( TPoint p, typename TPoint::Scalar s ); template <ScalarType TPoint> bool isLessEqual( TPoint const& lhs, TPoint const& rhs ); template <ScalarType TPoint> Expected<TPoint> tryGetElem( TPoint const& p, size_t row, size_t col = 0 ); template <ScalarType TPoint> Expected<Success> trySetElem( TPoint& p, TPoint s, size_t row, size_t col = 0 ); template <EigenDenseType TPoint> Expected<Success> trySetElem( TPoint& p, typename TPoint::Scalar s, size_t row, size_t col = 0 ); template <ScalarType TPoint> auto transpose(TPoint p); template <class TPoint, class TFunc, class TReduce> void reduceArg( TPoint const& x, TReduce& reduce, TFunc&& func ); template <class TPoint, class TFunc, class TReduce> void reduceArg( TPoint const& a, TPoint const& b, TReduce& reduce, TFunc&& func ); template <class TPoint, class TFunc, class TReduce> TReduce reduce( TPoint const& x, TReduce const& initial, TFunc&& func ); template <class TPoint, class TFunc, class TReduce> TReduce reduce( TPoint const& a, TPoint const& b, TReduce const& initial, TFunc&& func ); void processTestResult(bool passed); template <class TT> auto maxMetric(TT const& p0, TT const& p1); template <class TT> void setToZero(TT& p); template <class TT, class TScalar> void setElementAt(TT& p, TScalar value, int i); template <class TT> auto squaredNorm(TT const& p); template <class TT> auto transpose(TT const& p); Eigen::Hyperplane<double, 3> fitPlaneToPoints(Eigen::Matrix3Xd const& points); Eigen::Hyperplane<double, 3> fitPlaneToPoints(std::vector<Eigen::Vector3d> const& points); Eigen::Hyperplane<double, 3> fitPlaneToPoints(std::vector<Eigen::Vector3f> const& points); template <class TScalar> Eigen::Vector2<TScalar> normalFromSo2(So2<TScalar> const& foo_rotation_line); template <class TScalar> So2<TScalar> so2FromNormal(Eigen::Vector2<TScalar> normal_in_foo); template <class TScalar> Eigen::Vector3<TScalar> normalFromSo3(So3<TScalar> const& foo_rotation_plane); template <class TScalar> Eigen::Matrix3<TScalar> rotationFromNormal( Eigen::Vector3<TScalar> const& normal_in_foo, Eigen::Vector3<TScalar> x_dir_hint_foo = Eigen::Vector3<TScalar>(TScalar(1), TScalar(0), TScalar(0)), Eigen::Vector3<TScalar> y_dir_hint_foo = Eigen::Vector3<TScalar>(TScalar(0), TScalar(1), TScalar(0)) ); template <class TScalar> So3<TScalar> so3FromPlane(Eigen::Vector3<TScalar> const& normal_in_foo); template <class TScalar> Eigen::Hyperplane<TScalar, 2> lineFromSe2(Se2<TScalar> const& foo_from_line); template <class TScalar> Se2<TScalar> se2FromLine(Eigen::Hyperplane<TScalar, 2> const& line_in_foo); template <class TScalar> Eigen::Hyperplane<TScalar, 3> planeFromSe3(Se3<TScalar> const& foo_from_plane); template <class TScalar> Se3<TScalar> se3FromPlane(Eigen::Hyperplane<TScalar, 3> const& plane_in_foo); template <class TScalar, int kMatrixDim> Eigen::Hyperplane<TScalar, kMatrixDim> makeHyperplaneUnique(Eigen::Hyperplane<TScalar, kMatrixDim> const& plane); template <class TT> Eigen::Matrix<TT, 2, 1> proj(InverseDepthPoint3<TT> const& inverse_depth_point); template <class TT> Eigen::Matrix<TT, 2, 3> dxProjX(InverseDepthPoint3<TT> const&); template <class TT> Eigen::Matrix<TT, 2, 6> dxProjExpXPointAt0(InverseDepthPoint3<TT> const& inverse_depth_point); template <class TT> Eigen::Matrix<TT, 3, 1> scaledTransform( sophus::Se3<TT> const& foo_from_bar, InverseDepthPoint3<TT> const& inverse_depth_point_in_bar ); template <class TT> Eigen::Matrix<TT, 2, 1> projTransform( sophus::Se3<TT> const& foo_from_bar, InverseDepthPoint3<TT> const& inverse_depth_point_in_bar ); template <class TPoint> Eigen::Vector2<typename TPoint::Scalar> proj(Eigen::MatrixBase<TPoint> const& p); template <class TPoint> Eigen::Vector3<typename TPoint::Scalar> unproj( Eigen::MatrixBase<TPoint> const& p, const typename TPoint::Scalar& z = 1.0 ); Expected<UnitVector3F64> fromProto(proto::UnitVec3F64 const& proto); proto::UnitVec3F64 toProto(sophus::UnitVector3F64 const& uvec); Expected<Eigen::Hyperplane<double, 3>> fromProto(proto::Hyperplane3F64 const& proto); proto::Hyperplane3F64 toProto(Eigen::Hyperplane<double, 3> const& plane); template <class TT> Ray<TT, 2> operator*( Se2<TT> const& bar_from_foo, Ray<TT, 2> const& ray_foo ); template <class TT> Ray<TT, 3> operator*( Se3<TT> const& bar_from_foo, Ray<TT, 3> const& ray_foo ); template <class TT> Ray<TT, 3> operator*( Se3<TT> const& bar_from_foo, UnitVector<TT, 3> const& v_foo ); template <class TT> Ray2<TT> operator*( Sim2<TT> const& b_from_a, Ray2<TT> const& ray_a ); template <class TT> Ray3<TT> operator*( Sim3<TT> const& b_from_a, Ray3<TT> const& ray_a ); template <class TT> std::optional<ClosestApproachResult<TT>> closestApproachParameters( Ray3<TT> const& line_0, Ray3<TT> const& line_1 ); template <class TT> std::optional<Eigen::Vector3<TT>> closestApproach( Ray3<TT> const& line_0, Ray3<TT> const& line_1 ); template <class TScalar> UnitVector<TScalar, 2> operator*( So2<TScalar> const& bar_rotation_foo, UnitVector<TScalar, 2> const& v_foo ); template <class TScalar> UnitVector<TScalar, 3> operator*( So3<TScalar> const& bar_rotation_foo, UnitVector<TScalar, 3> const& v_foo ); template <class TScalar> UnitVector<TScalar, 2> operator*( RxSo2<TScalar> const& bar_rotscale_foo, UnitVector<TScalar, 2> const& v_foo ); template <class TScalar> UnitVector<TScalar, 3> operator*( RxSo3<TScalar> const& bar_rotscale_foo, UnitVector<TScalar, 3> const& v_foo ); template <class TScalar> SOPHUS_FUNC So3<TScalar> rotThroughPoints( UnitVector3<TScalar> const& from, UnitVector3<TScalar> const& to ); template <class TScalar> SOPHUS_FUNC So3<TScalar> rotThroughPoints( Eigen::Vector<TScalar, 3> const& from, Eigen::Vector<TScalar, 3> const& to ); ImageSize half(ImageSize image_size); bool operator==(ImageSize const& lhs, ImageSize const& rhs); bool operator!=( ImageSize const& lhs, ImageSize const& rhs ); bool operator<(ImageSize const& lhs, ImageSize const& rhs); std::ostream& operator<<(std::ostream& os, ImageSize const& image_size); bool operator==(ImageShape const& lhs, ImageShape const& rhs); bool operator!=( ImageShape const& lhs, ImageShape const& rhs ); std::ostream& operator<<(std::ostream& os, ImageShape const& shape); template <class TPixel> Region2I imageCoordsInterval( ImageSize image_size, int border = 0 ); int count(ImageViewBool mask, bool truth_value); int countTrue(ImageViewBool mask); int countFalse(ImageViewBool mask); bool isAllTrue(ImageViewBool mask); bool isAnyTrue(ImageViewBool mask); MutImageBool neg(ImageViewBool mask); std::optional<Eigen::Vector2i> firstPixel(ImageViewBool mask, bool truth_value); std::optional<Eigen::Vector2i> firstTruePixel(ImageViewBool mask); std::optional<Eigen::Vector2i> firstFalsePixel(ImageViewBool mask); SOPHUS_ENUM(NumberType, (fixed_point, floating_point)); template <class TPixel> MutImageBool isEqualMask(ImageView<TPixel> lhs, ImageView<TPixel> rhs); template <class TPixel> MutImageBool isLessMask(ImageView<TPixel> lhs, ImageView<TPixel> rhs); template <class TPixel> MutImageBool isGreaterMask(ImageView<TPixel> lhs, ImageView<TPixel> rhs); template <class TPixel> MutImageBool isNearMask( ImageView<TPixel> lhs, ImageView<TPixel> rhs, typename ImageTraits<TPixel>::ChannelT thr ); template <class TPixel> Region<TPixel> finiteInterval(sophus::ImageView<TPixel> const& image); template <class TPixel> Region2I imageCoordsInterval( sophus::ImageView<TPixel> const& image, int border = 0 ); template <class TT> TT interpolate( sophus::ImageView<TT> const& image, Eigen::Vector2f uv ); sophus::ImageSize fromProto(proto::ImageSize const& proto); proto::ImageSize toProto(sophus::ImageSize const& image_size); bool operator==( RuntimePixelType const& lhs, RuntimePixelType const& rhs ); std::ostream& operator<<(std::ostream& os, RuntimePixelType const& type); template < typename TUserFunc, class TPredicate = IntensityImagePredicate, template<class> class TAllocator = Eigen::aligned_allocator > void visitImage( TUserFunc&& func, RuntimeImage<TPredicate, TAllocator> const& image ); template <class TUserFunc, class TPredicate = IntensityImagePredicate> void visitImage( TUserFunc&& func, RuntimeImageView<TPredicate> const& image ); int testCartesian(); template <class TScalar> Eigen::Hyperplane<TScalar, 2> through(Eigen::Vector<TScalar, 2> const* points); template <class TScalar> Eigen::Hyperplane<TScalar, 3> through(Eigen::Vector<TScalar, 3> const* points); template <class TScalar> std::vector<Se3<TScalar>, Eigen::aligned_allocator<Se3<TScalar>>> getTestSE3s(); template <class TT> std::vector<Se2<TT>, Eigen::aligned_allocator<Se2<TT>>> getTestSE2s(); template <class TSequenceContainer> std::optional<typename TSequenceContainer::value_type> iterativeMean( TSequenceContainer const& foo_transforms_bar, int max_num_iterations ); template < class TSequenceContainer, int kPointDim = TSequenceContainer::value_type::kDoF, class TScalar = typename TSequenceContainer::value_type::Scalar > std::enable_if_t<std::is_same<typename TSequenceContainer::value_type, Cartesian<TScalar, kPointDim>>::value, std::optional<typename TSequenceContainer::value_type>> average(TSequenceContainer const& foo_transforms_bar); template < class TSequenceContainer, class TScalar = typename TSequenceContainer::value_type::Scalar > std::enable_if_t<std::is_same<typename TSequenceContainer::value_type, So2<TScalar>>::value, std::optional<typename TSequenceContainer::value_type>> average(TSequenceContainer const& foo_transforms_bar); template < class TSequenceContainer, class TScalar = typename TSequenceContainer::value_type::Scalar > std::enable_if_t<std::is_same<typename TSequenceContainer::value_type, RxSo2<TScalar>>::value, std::optional<typename TSequenceContainer::value_type>> average(TSequenceContainer const& foo_transforms_bar); template < class TSequenceContainer, class TScalar = typename TSequenceContainer::value_type::Scalar > std::enable_if_t<std::is_same<typename TSequenceContainer::value_type, So3<TScalar>>::value, std::optional<typename TSequenceContainer::value_type>> average(TSequenceContainer const& foo_transforms_bar); template < class TSequenceContainer, class TScalar = typename TSequenceContainer::value_type::Scalar > std::enable_if_t<std::is_same<typename TSequenceContainer::value_type, RxSo3<TScalar>>::value, std::optional<typename TSequenceContainer::value_type>> average(TSequenceContainer const& foo_transforms_bar); template < class TSequenceContainer, class TScalar = typename TSequenceContainer::value_type::Scalar > std::enable_if_t<std::is_same<typename TSequenceContainer::value_type, Se2<TScalar>>::value, std::optional<typename TSequenceContainer::value_type>> average( TSequenceContainer const& foo_transforms_bar, int max_num_iterations = 20 ); template < class TSequenceContainer, class TScalar = typename TSequenceContainer::value_type::Scalar > std::enable_if_t<std::is_same<typename TSequenceContainer::value_type, Sim2<TScalar>>::value, std::optional<typename TSequenceContainer::value_type>> average( TSequenceContainer const& foo_transforms_bar, int max_num_iterations = 20 ); template < class TSequenceContainer, class TScalar = typename TSequenceContainer::value_type::Scalar > std::enable_if_t<std::is_same<typename TSequenceContainer::value_type, Se3<TScalar>>::value, std::optional<typename TSequenceContainer::value_type>> average( TSequenceContainer const& foo_transforms_bar, int max_num_iterations = 20 ); template < class TSequenceContainer, class TScalar = typename TSequenceContainer::value_type::Scalar > std::enable_if_t<std::is_same<typename TSequenceContainer::value_type, Sim3<TScalar>>::value, std::optional<typename TSequenceContainer::value_type>> average( TSequenceContainer const& foo_transforms_bar, int max_num_iterations = 20 ); template <class TGroup, class TScalar2 = typename TGroup::Scalar> std::enable_if_t<interp_details::Traits<TGroup>::kSupported, TGroup> interpolate( TGroup const& foo_transform_bar, TGroup const& foo_transform_daz, TScalar2 p = TScalar2(0.5f) ); Eigen::Quaterniond fromProto(proto::QuaternionF64 const& proto); proto::QuaternionF64 toProto(Eigen::Quaterniond const& quat); Expected<So3F64> fromProto(proto::So3F64 const& proto); proto::So3F64 toProto(sophus::So3F64 const& rotation); Expected<sophus::Se3F64> fromProto(proto::Se3F64 const& proto); proto::Se3F64 toProto(Se3F64 const& pose); int testRxso2(); int testRxso3(); int testSe2(); int testSe3(); int testSim3(); int testSo2(); int testSo3(); Eigen::Matrix<uint32_t, 2, 1> fromProto(proto::Vec2I64 const& proto); proto::Vec2I64 toProto(Eigen::Matrix<uint32_t, 2, 1> const& v); Eigen::Vector2f fromProto(proto::Vec2F32 const& proto); proto::Vec2F32 toProto(Eigen::Vector2f const& v); Eigen::Vector2d fromProto(proto::Vec2F64 const& proto); proto::Vec2F64 toProto(Eigen::Vector2d const& v); Eigen::Matrix<uint32_t, 3, 1> fromProto(proto::Vec3I64 const& proto); proto::Vec3I64 toProto(Eigen::Matrix<uint32_t, 3, 1> const& v); Eigen::Vector3f fromProto(proto::Vec3F32 const& proto); proto::Vec3F32 toProto(Eigen::Vector3f const& v); Eigen::Vector3d fromProto(proto::Vec3F64 const& proto); proto::Vec3F64 toProto(Eigen::Vector3d const& v); template <class TD> SOPHUS_FUNC bool isOrthogonal(Eigen::MatrixBase<TD> const& r); template <class TD> SOPHUS_FUNC bool isScaledOrthogonalAndPositive(Eigen::MatrixBase<TD> const& s_r); template <class TD> SOPHUS_FUNC std::enable_if_t<std::is_floating_point<typename TD::Scalar>::value, Eigen::Matrix<typename TD::Scalar, TD::RowsAtCompileTime, TD::RowsAtCompileTime>> makeRotationMatrix(Eigen::MatrixBase<TD> const& r); PinholeModel createDefaultPinholeModel(ImageSize image_size); OrthographicModel createDefaultOrthoModel(ImageSize image_size); template <class TScalar> Eigen::Matrix<TScalar, 2, 1> subsampleDown(Eigen::Matrix<TScalar, 2, 1> const& in); template <class TScalar> Eigen::Matrix<TScalar, 2, 1> subsampleUp(Eigen::Matrix<TScalar, 2, 1> const& in); template <class TScalar> Eigen::Matrix<TScalar, 2, 1> binDown(Eigen::Matrix<TScalar, 2, 1> const& in); template <class TScalar> Eigen::Matrix<TScalar, 2, 1> binUp(Eigen::Matrix<TScalar, 2, 1> const& in); SOPHUS_ENUM( CameraDistortionType, (pinhole, brown_conrady, kannala_brandt_k3, orthographic) ); GyroModelVariant getModelFromType( GyroModelType model_type, Eigen::VectorXd const& params ); AcceleroModelVariant getModelFromType( AcceleroModelType model_type, Eigen::VectorXd const& params ); template <class TT> Eigen::Matrix<TT, 3, 3> nonOrthogonalityMatrix(Eigen::Matrix<TT, 3, 1> const& non_orthogonality); SOPHUS_ENUM( GyroModelType, (scaling_non_orthogonality) ); SOPHUS_ENUM( AcceleroModelType, (scaling_non_orthogonality) ); template <class TScalar> OrthographicModelT<TScalar> orthoCamFromBoundingBox(Region2<TScalar> const& bounding_box, ImageSize image_size); template <class TScalar> Region2<TScalar> boundingBoxFromOrthoCam(OrthographicModelT<TScalar> const& ortho_cam); Expected<CameraModel> fromProto(proto::CameraModel const& proto); proto::CameraModel toProto(CameraModel const& camera_model); Expected<std::vector<CameraModel>> fromProto(proto::CameraModels const& proto); proto::CameraModels toProto(std::vector<CameraModel> const& camera_models); } // namespace sophus
Detailed Documentation¶
Image MutImage, owning images types.
ImageView and MutImageView, non owning images types.
Note that it is a conscious API decision to follow “shallow-compare” type semantic for ImageView, MutImageView, Image and MutImage. See image_view.h
for details.
See image.h
for Image and MutImage, owning images types.
Note that it is a conscious API decision to follow “shallow-compare” type semantic for ImageView, MutImageView, Image and MutImage. Similar “shallow-compare” types are: std::span (shallow-compare reference type), and std::unique_ptr (shallow compare unique ownership type). This is in contrast to regular types such as std::vector, std::string and reference types which mimic regular type semantic such as std::string_view. Also see https://abseil.io/blog/20180531-regular-types.
Typedefs¶
using IsMappable = IsSpecialized<Eigen::internal::traits<std::decay_t<TScalar>>>
Type trait used to distinguish mappable vector types from scalars.
We use this class to distinguish Eigen::Vector<Scalar, kMatrixDim> from Scalar types in LieGroup<TScalar>::Tangent
Primary use is mapping LieGroup::Tangent over raw data, with 2 options:
LieGroup::Tangent is “scalar” (for So2), then we just dereference pointer
LieGroup::Tangent is Eigen::Vector<…>, then we need to use Eigen::Map
Specialization of Eigen::internal::traits<TScalar> for TScalar is crucial for for constructing Eigen::Map<TScalar>, thus we use that property for distinguishing between those two options. At this moment there seem to be no option to check this using only “external” API of Eigen
using AnyImage = RuntimeImage<AnyImagePredicate, TAllocator>
Image representing any number of channels (>=1) and any floating and unsigned integral channel type.
using IntensityImage = RuntimeImage<IntensityImagePredicate, TAllocator>
Image to represent intensity image / texture as grayscale (=1 channel), RGB (=3 channel ) and RGBA (=4 channel), either uint8_t [0-255], uint16 [0-65535] or float [0.0-1.0] channel type.
using PinholeModel = CameraModelT<double, AffineTransform, ProjectionZ1>
Pinhole camera model.
using BrownConradyModel = CameraModelT<double, BrownConradyTransform, ProjectionZ1>
Brown Conrady camera model.
using KannalaBrandtK3Model = CameraModelT<double, KannalaBrandtK3Transform, ProjectionZ1>
KannalaBrandt camera model with k0, k1, k2, k3.
using CameraDistortionVariant = std::variant<PinholeModel, BrownConradyModel, KannalaBrandtK3Model, OrthographicModel>
Variant of camera models.
Global Functions¶
template <class TScalar, class TFn> auto curveNumDiff( TFn curve, TScalar t, TScalar h = kEpsilonSqrt<TScalar> )
Calculates the derivative of a curve at a point t
.
Here, a curve is a function from a Scalar to a Euclidean space. Thus, it returns either a Scalar, a vector or a matrix.
template <class TScalar, int kMatrixDim, int kM, class TScalarOrVector, class TFn> Eigen::Matrix<TScalar, kMatrixDim, kM> vectorFieldNumDiff( TFn vector_field, TScalarOrVector const& a, TScalar eps = kEpsilonSqrt<TScalar> )
Calculates the derivative of a vector field at a point a
.
Here, a vector field is a function from a vector space to another vector space.
template <class TT> auto maxMetric(TT const& p0, TT const& p1)
Returns maximum metric between two points p0
and p1
, with p0, p1
being matrices or a scalars.
template <class TT> void setToZero(TT& p)
Sets point p
to zero, with p
being a matrix or a scalar.
template <class TT, class TScalar> void setElementAt( TT& p, TScalar value, int i )
Sets i
th component of p
to value
, with p
being a matrix or a scalar. If p
is a scalar, i
must be 0
.
template <class TT> auto squaredNorm(TT const& p)
Returns the squared 2-norm of p
, with p
being a vector or a scalar.
template <class TT> auto transpose(TT const& p)
Returns p.transpose()
if p
is a matrix, and simply p
if m is a scalar.
template <class TScalar> Eigen::Vector2<TScalar> normalFromSo2(So2<TScalar> const& foo_rotation_line)
Takes in a rotation foo_rotation_plane
and returns the corresponding line normal along the y-axis (in reference frame foo
).
template <class TScalar> So2<TScalar> so2FromNormal(Eigen::Vector2<TScalar> normal_in_foo)
Takes in line normal in reference frame foo and constructs a corresponding rotation matrix foo_rotation_line
.
Precondition: normal_in_foo
must not be close to zero.
template <class TScalar> Eigen::Vector3<TScalar> normalFromSo3(So3<TScalar> const& foo_rotation_plane)
Takes in a rotation foo_rotation_plane
and returns the corresponding plane normal along the z-axis (in reference frame foo
).
template <class TScalar> Eigen::Matrix3<TScalar> rotationFromNormal( Eigen::Vector3<TScalar> const& normal_in_foo, Eigen::Vector3<TScalar> x_dir_hint_foo = Eigen::Vector3<TScalar>(TScalar(1), TScalar(0), TScalar(0)), Eigen::Vector3<TScalar> y_dir_hint_foo = Eigen::Vector3<TScalar>(TScalar(0), TScalar(1), TScalar(0)) )
Takes in plane normal in reference frame foo and constructs a corresponding rotation matrix foo_rotation_plane
.
Note: The plane
frame is defined as such that the normal points along the positive z-axis. One can specify hints for the x-axis and y-axis of the plane
frame.
Preconditions:
normal_in_foo
,xDirHint_foo
,yDirHint_foo
must not be close to zero.xDirHint_foo
andyDirHint_foo
must be approx. perpendicular.
template <class TScalar> So3<TScalar> so3FromPlane(Eigen::Vector3<TScalar> const& normal_in_foo)
Takes in plane normal in reference frame foo and constructs a corresponding rotation matrix foo_rotation_plane
.
See rotationFromNormal
for details.
template <class TScalar> Eigen::Hyperplane<TScalar, 2> lineFromSe2(Se2<TScalar> const& foo_from_line)
Returns a line (wrt. to frame foo
), given a pose of the line
in reference frame foo
.
Note: The plane is defined by X-axis of the line
frame.
template <class TScalar> Se2<TScalar> se2FromLine(Eigen::Hyperplane<TScalar, 2> const& line_in_foo)
Returns the pose T_foo_line
, given a line in reference frame foo
.
Note: The line is defined by X-axis of the frame line
.
template <class TScalar> Eigen::Hyperplane<TScalar, 3> planeFromSe3(Se3<TScalar> const& foo_from_plane)
Returns a plane (wrt. to frame foo
), given a pose of the plane
in reference frame foo
.
Note: The plane is defined by XY-plane of the frame plane
.
template <class TScalar> Se3<TScalar> se3FromPlane(Eigen::Hyperplane<TScalar, 3> const& plane_in_foo)
Returns the pose foo_from_plane
, given a plane in reference frame foo
.
Note: The plane is defined by XY-plane of the frame plane
.
template <class TScalar, int kMatrixDim> Eigen::Hyperplane<TScalar, kMatrixDim> makeHyperplaneUnique(Eigen::Hyperplane<TScalar, kMatrixDim> const& plane)
Takes in a hyperplane and returns unique representation by ensuring that the offset
is not negative.
template <class TT> Eigen::Matrix<TT, 2, 1> proj(InverseDepthPoint3<TT> const& inverse_depth_point)
Projects 3-point (a,b,psi) = (x/z,y/z,1/z) through the origin (0,0,0) onto the plane z=1. Hence it returns (a,b) = (x/z, y/z).
template <class TT> Eigen::Matrix<TT, 2, 3> dxProjX(InverseDepthPoint3<TT> const&)
Returns point derivative of inverse depth point projection:
Dx proj(x) with x = (a,b,psi) being an inverse depth point.
template <class TT> Eigen::Matrix<TT, 2, 6> dxProjExpXPointAt0(InverseDepthPoint3<TT> const& inverse_depth_point)
Returns pose derivative of inverse depth point projection at the identity:
Dx proj(exp(x) * y) at x=0
with y = (a,b,psi) being an inverse depth point.
template <class TT> Eigen::Matrix<TT, 3, 1> scaledTransform( sophus::Se3<TT> const& foo_from_bar, InverseDepthPoint3<TT> const& inverse_depth_point_in_bar )
Transforms inverse_depth point in frame bar to a scaled inverse depth point in frame foo. Here the scale is psi, the input inverse depth.
Given (a,b,psi) being the inverse depth point in frame bar, it returns
psi * (foo_from_bar * inverse_depth_point_in_bar.toEuclideanPoint3())
for psi!=0.
template <class TT> Eigen::Matrix<TT, 2, 1> projTransform( sophus::Se3<TT> const& foo_from_bar, InverseDepthPoint3<TT> const& inverse_depth_point_in_bar )
Transforms inverse_depth point from frame bar to frame foo followed by a projection.
If psi != 0, hence the point is not at +/- infinity, this function is equivalent to:
camProj(foo_from_bar * inverse_depth_point_in_bar.toEuclideanPoint3());
However, this function can also applied when 1/z==0, hence the point is at +/- infinity.
template <class TPoint> Eigen::Vector2<typename TPoint::Scalar> proj(Eigen::MatrixBase<TPoint> const& p)
Projects 3-point (x,y,z) through the origin (0,0,0) onto the plane z=1. Hence it returns (x/z, y/z).
Precondition: z must not be close to 0.
template <class TPoint> Eigen::Vector3<typename TPoint::Scalar> unproj( Eigen::MatrixBase<TPoint> const& p, const typename TPoint::Scalar& z = 1.0 )
Maps point on the z=1 plane (a,b) to homogeneous representation of the same point: (z*a, z*b, z). Z defaults to 1.
template <class TT> std::optional<ClosestApproachResult<TT>> closestApproachParameters( Ray3<TT> const& line_0, Ray3<TT> const& line_1 )
For two parametric lines in lambda0 and lambda1 respectively,.
line_0: x(lambda0) = o0 + lambda0 * d0 line_1: y(lambda1) = o1 + lambda1 * d1
returns distances [lambda0, lambda1] along the respective rays, corresponding to the closest approach of x and y according to an l2 distance measure. lambda0 and lambda1 may be positive or negative. If line_0 and line_1 are parallel, returns nullopt as no unique solution exists
template <class TT> std::optional<Eigen::Vector3<TT>> closestApproach( Ray3<TT> const& line_0, Ray3<TT> const& line_1 )
For two lines line_0
and line_1
returns the mid-point of the line segment connecting one point from each of the lines which are closest to one another according to the l2 distance measure.
If line_0 and line_1 are parallel, returns nullopt as no unique solution exists
template <class TScalar> SOPHUS_FUNC So3<TScalar> rotThroughPoints( UnitVector3<TScalar> const& from, UnitVector3<TScalar> const& to )
Construct rotation which would take unit direction vector from
into to
such that to = rotThroughPoints(from,to) * from
. I.e. that the rotated point from
is colinear with to
(equal up to scale)
The axis of rotation is perpendicular to both from
and to
.
template <class TScalar> SOPHUS_FUNC So3<TScalar> rotThroughPoints( Eigen::Vector<TScalar, 3> const& from, Eigen::Vector<TScalar, 3> const& to )
Construct rotation which would take direction vector from
into to
such that to \propto rotThroughPoints(from,to) * from
. I.e. that the rotated point from
is colinear with to
(equal up to scale)
The axis of rotation is perpendicular to both from
and to
.
Precondition: Neither from
nor to
must be zero. This is unchecked.
ImageSize half(ImageSize image_size)
If the original width [height] is odd, the new width [height] will be: (width+1)/2 [height+1)/2].
bool operator==(ImageSize const& lhs, ImageSize const& rhs)
Equality operator.
bool operator<(ImageSize const& lhs, ImageSize const& rhs)
Ordering operator, for keys in sets and maps.
std::ostream& operator<<(std::ostream& os, ImageSize const& image_size)
Ostream operator.
bool operator==(ImageShape const& lhs, ImageShape const& rhs)
Equality operator.
std::ostream& operator<<(std::ostream& os, ImageShape const& shape)
Ostream operator.
int count(ImageViewBool mask, bool truth_value)
Returns number of pixels equal truth_value
in mask.
int countTrue(ImageViewBool mask)
Returns number of true pixels in mask.
int countFalse(ImageViewBool mask)
Returns number of false pixels in mask.
bool isAllTrue(ImageViewBool mask)
Returns true if all pixels are true.
bool isAnyTrue(ImageViewBool mask)
Returns true if at least one pixel is true.
MutImageBool neg(ImageViewBool mask)
Returns boolean image with the result per pixel:
out_mask(..) = !mask(..)
std::optional<Eigen::Vector2i> firstPixel(ImageViewBool mask, bool truth_value)
Returns first pixel of mask which equals truth_value
, nullopt otherwise.
std::optional<Eigen::Vector2i> firstTruePixel(ImageViewBool mask)
Returns first true pixel, nullopt otherwise.
std::optional<Eigen::Vector2i> firstFalsePixel(ImageViewBool mask)
Returns first false pixel, nullopt otherwise.
SOPHUS_ENUM(NumberType, (fixed_point, floating_point))
Number type.
template <class TPixel> MutImageBool isEqualMask(ImageView<TPixel> lhs, ImageView<TPixel> rhs)
Returns boolean image with the result per pixel:
mask(..) = lhs(..) == rhs (..)
template <class TPixel> MutImageBool isLessMask(ImageView<TPixel> lhs, ImageView<TPixel> rhs)
Returns boolean image with the result per pixel:
mask(..) = lhs(..) < rhs (..)
template <class TPixel> MutImageBool isGreaterMask(ImageView<TPixel> lhs, ImageView<TPixel> rhs)
Returns boolean image with the result per pixel:
mask(..) = lhs(..) > rhs (..)
template <class TPixel> MutImageBool isNearMask( ImageView<TPixel> lhs, ImageView<TPixel> rhs, typename ImageTraits<TPixel>::ChannelT thr )
Returns boolean image with the result per pixel:
mask(..) = ||lhs(..), rhs (..)|| <= thr
std::ostream& operator<<(std::ostream& os, RuntimePixelType const& type)
Example: RuntimePixelType::fromTemplate<float>() outputs: “1F32”; RuntimePixelType::fromTemplate <Eigen::Matrix<uint8_t,4,1>>() outputs: “4U8”;.
template <class TSequenceContainer> std::optional<typename TSequenceContainer::value_type> iterativeMean( TSequenceContainer const& foo_transforms_bar, int max_num_iterations )
Calculates mean iteratively.
Returns nullopt
if it does not converge.
template <class TGroup, class TScalar2 = typename TGroup::Scalar> std::enable_if_t<interp_details::Traits<TGroup>::kSupported, TGroup> interpolate( TGroup const& foo_transform_bar, TGroup const& foo_transform_daz, TScalar2 p = TScalar2(0.5f) )
This function interpolates between two Lie group elements foo_transform_bar
and foo_transform_daz
with an interpolation factor of alpha
in [0, 1].
It returns a pose foo_T_quiz
with quiz
being a frame between bar
and baz
. If alpha=0
it returns foo_transform_bar
. If it is 1, it returns foo_transform_daz
.
(Since interpolation on Lie groups is inverse-invariant, we can equivalently think of the input arguments as being bar_transform_foo
, baz_transform_foo
and the return value being quiz_T_foo
.)
Precondition: p
must be in [0, 1].
template <class TD> SOPHUS_FUNC bool isOrthogonal(Eigen::MatrixBase<TD> const& r)
Takes in arbitrary square matrix and returns true if it is orthogonal.
template <class TD> SOPHUS_FUNC bool isScaledOrthogonalAndPositive(Eigen::MatrixBase<TD> const& s_r)
Takes in arbitrary square matrix and returns true if it is “scaled-orthogonal” with positive determinant.
template <class TD> SOPHUS_FUNC std::enable_if_t<std::is_floating_point<typename TD::Scalar>::value, Eigen::Matrix<typename TD::Scalar, TD::RowsAtCompileTime, TD::RowsAtCompileTime>> makeRotationMatrix(Eigen::MatrixBase<TD> const& r)
Takes in arbitrary square matrix (2x2 or larger) and returns closest orthogonal matrix with positive determinant.
PinholeModel createDefaultPinholeModel(ImageSize image_size)
Creates default pinhole model from image_size
.
OrthographicModel createDefaultOrthoModel(ImageSize image_size)
Creates default orthographic model from image_size
.
template <class TScalar> Eigen::Matrix<TScalar, 2, 1> subsampleDown(Eigen::Matrix<TScalar, 2, 1> const& in)
Subsamples pixel down, factor of 0.5.
See for details: https://docs.google.com/document/d/1xmhCMWklP2UoQMGaMqFnsoPWoeMvBfXN7S8-ie9k0UA/edit#heading=h.97r8rr8owwpc
template <class TScalar> Eigen::Matrix<TScalar, 2, 1> subsampleUp(Eigen::Matrix<TScalar, 2, 1> const& in)
Subsamples pixel up, factor of 2.0.
See for details: https://docs.google.com/document/d/1xmhCMWklP2UoQMGaMqFnsoPWoeMvBfXN7S8-ie9k0UA/edit#heading=h.97r8rr8owwpc
template <class TScalar> Eigen::Matrix<TScalar, 2, 1> binDown(Eigen::Matrix<TScalar, 2, 1> const& in)
Bins pixel down, factor of 0.5.
See for details: https://docs.google.com/document/d/1xmhCMWklP2UoQMGaMqFnsoPWoeMvBfXN7S8-ie9k0UA/edit#heading=h.elfm6123mecj
template <class TScalar> Eigen::Matrix<TScalar, 2, 1> binUp(Eigen::Matrix<TScalar, 2, 1> const& in)
Bins pixel up, factor of 2.0.
See for details: https://docs.google.com/document/d/1xmhCMWklP2UoQMGaMqFnsoPWoeMvBfXN7S8-ie9k0UA/edit#heading=h.elfm6123mecj
SOPHUS_ENUM( CameraDistortionType, (pinhole, brown_conrady, kannala_brandt_k3, orthographic) )
Camera model projection type.
template <class TScalar> OrthographicModelT<TScalar> orthoCamFromBoundingBox( Region2<TScalar> const& bounding_box, ImageSize image_size )
Returns orthographic camera model given bounding box and image size.
template <class TScalar> Region2<TScalar> boundingBoxFromOrthoCam(OrthographicModelT<TScalar> const& ortho_cam)
Returns 2d bounding box corresponding the the given orthographic camera model.