rmcv
v0.1.0
A library for RoboMaster computer vision based on OpenCV.
|
Main modules. More...
Namespaces | |
debug | |
Debug utilities. | |
Classes | |
class | Armour |
union | BitFloat |
class | DahengCamera |
class | LightBlob |
class | Package |
class | ParallelQueue |
class | SerialPort |
Typedefs | |
typedef std::vector< cv::Point > | Contour |
typedef enum rm::CompensateMode | CompensateMode |
Define witch method is used to calculate the compensation of gravity. | |
Enumerations | |
enum | CampType { CAMP_RED = 0, CAMP_BLUE = 1, CAMP_GUIDELIGHT = 2, CAMP_NEUTRAL = -1 } |
enum | AimMode { AIM_COMBAT = 0, AIM_BUFF = 1, AIM_SNIPE = 2 } |
enum | RectType { RECT_TALL = 0, RECT_SIDE = 1 } |
enum | FileType { FILETYPE_REGULAR_FILE = 0, FILETYPE_DIRECTORY = 1, FILETYPE_SYMBOLIC_LINK = 2, FILETYPE_SOCKET = 3, FILETYPE_UNKNOWN = -1 } |
enum | CompensateMode { COMPENSATE_NONE = 0, COMPENSATE_CLASSIC = 1, COMPENSATE_NI = 2 } |
Define witch method is used to calculate the compensation of gravity. More... | |
Functions | |
rm::FileType | GetFileType (const char *filename) |
bool | ListFiles (const char *path, std::vector< std::string > &filenames) |
cv::Rect | GetROI (cv::Point2f *imagePoints, int pointsCount, float scaleFactor=1.0f, const cv::Size &frameSize={-1, -1}, const cv::Rect &previous={0, 0, 0, 0}) |
cv::Rect | GetROI (cv::Point2f *imagePoints, int pointsCount, const cv::Size2f &scaleFactor={1, 1}, const cv::Size &frameSize={-1, -1}, const cv::Rect &previous={0, 0, 0, 0}) |
void | VerticesRectify (cv::RotatedRect &input, cv::Point2f *output, RectType type) |
void | CalcPerspective (cv::Point2f input[4], cv::Point2f output[4], float outRatio=1.0f) |
double | NewtonIteration (double(*fd)(double), double x0=0, double error=0.0001, int cycle=1024) |
double | NewtonIteration (double(*fd)(double, std::vector< double >), const std::vector< double > &literals, double x0=0, double error=0.0001, int cycle=1024) |
double | ProjectileMotionFD (double theta, std::vector< double > literals) |
float | PointDistance (const cv::Point2f &pt1, const cv::Point2f &pt2) |
float | PointDistance (const cv::Point2i &pt1, const cv::Point2i &pt2) |
cv::Point2f | LineCenter (const cv::Point2f &pt1, const cv::Point2f &pt2) |
void | ExtendCord (const cv::Point2f &pt1, const cv::Point2f &pt2, float deltaLen, cv::Point2f &dst1, cv::Point2f &dst2) |
std::string | PathCombine (const std::string &path1, const std::string &path2) |
std::string | int2str (int number) |
void | CalcRatio (cv::Mat &source, cv::Mat &calibration, cv::Point2f vertices[4], cv::Size outSize) |
void | CalcGamma (cv::Mat &source, cv::Mat &calibration, float gamma=0.5f) |
void | ExtractColor (cv::InputArray image, cv::OutputArray binary, rm::CampType enemy, int lowerBound, bool overExposed=false, cv::Size kernelSize={0, 0}) |
void | AutoEnhance (cv::Mat &frame, float maxGainFactor=100.0, float minGainFactor=50.0) |
void | AutoBinarize (cv::Mat &image, cv::Mat &binary) |
void | AxisRotateX (double y, double z, double thetaX, double &outY, double &outZ) |
Rotate the vector around the x-axis. More... | |
void | AxisRotateY (double x, double z, double thetaY, double &outX, double &outZ) |
Rotate the vector around the y-axis. More... | |
void | AxisRotateZ (double x, double y, double thetaZ, double &outX, double &outY) |
Rotate the vector around the z-axis. More... | |
double | DeltaHeight (cv::InputArray translationVector, double motorAngle, const cv::Point2f &offset={0, 0}, double angleOffset=0) |
Solve height difference between barrel and target. More... | |
double | Distance (cv::InputArray translationVector) |
Solve the distance between camera and target. More... | |
double | ProjectileAngle (double v0, double g, double d, double h) |
Solve initial angle required for a projectile motion. More... | |
bool | SolveCameraPose (cv::InputArray rotationVector, cv::InputArray translationVector, cv::OutputArray pose) |
Solve camera pose in pitch, yaw, and roll relative to a target. More... | |
double | SolveGEA (cv::InputArray translationVector, cv::OutputArray gimbalErrorAngle, double g, double v0, double h, const cv::Point2f &offset={0, 0}, double angleOffset=0, rm::CompensateMode mode=rm::COMPENSATE_NONE) |
Solve gimbal error angle to target by given method. More... | |
bool | SolvePNP (const std::vector< cv::Point2f > &imagePoints, cv::InputArray cameraMatrix, cv::InputArray distortionFactor, const cv::Size2f &exactSize, cv::OutputArray translationVector, cv::OutputArray rotationVector, const cv::Rect &ROI={0, 0, 0, 0}) |
Solve the rotation & translation vector using cv::solvePnP & cv::SOLVEPNP_IPPE_SQUARE. More... | |
bool | MatchLightBlob (const rm::Contour &contour, float minRatio, float maxRatio, float tiltAngle, float minArea, float maxArea, cv::RotatedRect &lightBlobBox, bool fitEllipse=true) |
void | FindLightBlobs (std::vector< rm::Contour > &contours, std::vector< rm::LightBlob > &lightBlobs, float minRatio, float maxRatio, float tiltAngle, float minArea, float maxArea, const cv::Mat &source, bool fitEllipse=true) |
void | FindLightBlobs (std::vector< rm::Contour > &contours, std::vector< rm::LightBlob > &lightBlobs, float minRatio, float maxRatio, float tiltAngle, float minArea, float maxArea, rm::CampType camp, bool fitEllipse=true) |
int | FindGuidLight (const std::vector< rm::LightBlob > &lightBlobs, const cv::Mat &source) |
bool | LightBlobOverlap (std::vector< rm::LightBlob > &lightBlobs, int leftIndex, int rightIndex) |
void | FindArmour (std::vector< rm::LightBlob > &lightBlobs, std::vector< rm::Armour > &armours, float maxAngleDif, float errAngle, float minBoxRatio, float maxBoxRatio, float lenRatio, rm::CampType enemy, bool filter=true) |
unsigned char | LookupCRC (unsigned char *data, unsigned char dataLength, const unsigned char *crcTable=rm::CRC8) |
Variables | |
const unsigned char | CRC8 [256] |
Main modules.
enum rm::CompensateMode |
void rm::AutoBinarize | ( | cv::Mat & | image, |
cv::Mat & | binary | ||
) |
Use the mean value of each channels to binarize given image and normalize to CV_32FC1.
image | Source image. |
binary | Destine image. |
void rm::AutoEnhance | ( | cv::Mat & | frame, |
float | maxGainFactor = 100.0 , |
||
float | minGainFactor = 50.0 |
||
) |
Auto enhance image by the given benchmarks.
frame | Source image & destine image. |
maxGainFactor | The mean value of pixel values where gain should be maximize. |
minGainFactor | The mean value of pixel values where gain should be minimize. |
void rm::AxisRotateX | ( | double | y, |
double | z, | ||
double | thetaX, | ||
double & | outY, | ||
double & | outZ | ||
) |
Rotate the vector around the x-axis.
y | y of the vector. |
z | z of the vector. |
thetaX | angle to be rotate. |
outY | y of the vector after rotation. |
outZ | z of the vector after rotation. |
void rm::AxisRotateY | ( | double | x, |
double | z, | ||
double | thetaY, | ||
double & | outX, | ||
double & | outZ | ||
) |
Rotate the vector around the y-axis.
x | x of the vector. |
z | z of the vector. |
thetaY | angle to be rotate. |
outX | x of the vector after rotation. |
outZ | z of the vector after rotation. |
void rm::AxisRotateZ | ( | double | x, |
double | y, | ||
double | thetaZ, | ||
double & | outX, | ||
double & | outY | ||
) |
Rotate the vector around the z-axis.
x | x of the vector. |
y | y of the vector. |
thetaZ | angle to be rotate. |
outX | x of the vector after rotation. |
outY | y of the vector after rotation. |
void rm::CalcGamma | ( | cv::Mat & | source, |
cv::Mat & | calibration, | ||
float | gamma = 0.5f |
||
) |
Perform a gamma transform on the input image.
source | Source image. |
calibration | Output calibrated image. |
gamma | Gamma factor. |
void rm::CalcPerspective | ( | cv::Point2f | input[4], |
cv::Point2f | output[4], | ||
float | outRatio = 1.0f |
||
) |
Calibrate given point sets in to specified aspect ratio.
input | Origin point sets. |
output | Points sets in specified aspect ratio. |
outRatio | Aspect ratio. |
void rm::CalcRatio | ( | cv::Mat & | source, |
cv::Mat & | calibration, | ||
cv::Point2f | vertices[4], | ||
cv::Size | outSize | ||
) |
Calibrate a portion of the source frame to the destine rect.
source | Source image. |
calibration | Output calibrated image. |
vertices | Vertices of the portion on the source frame. |
outSize | Destine size. |
double rm::DeltaHeight | ( | cv::InputArray | translationVector, |
double | motorAngle, | ||
const cv::Point2f & | offset = {0, 0} , |
||
double | angleOffset = 0 |
||
) |
Solve height difference between barrel and target.
translationVector | The translation vector of target. |
motorAngle | The motor angle of gimbal, positive upwards. (RAD) |
offset | Offset between camera and barrel. (cm) |
angleOffset | Angle offset between camera and barrel. (RAD) |
double rm::Distance | ( | cv::InputArray | translationVector | ) |
Solve the distance between camera and target.
translationVector | The translation vector of target. |
void rm::ExtendCord | ( | const cv::Point2f & | pt1, |
const cv::Point2f & | pt2, | ||
float | deltaLen, | ||
cv::Point2f & | dst1, | ||
cv::Point2f & | dst2 | ||
) |
Expand the cord by the given length while center of the cord remain still.
pt1 | First point of the cord. |
pt2 | Second point of the cord. |
deltaLen | The length to be expanded. |
dst1 | The first point after expanding. |
dst2 | The second point after expanding. |
void rm::ExtractColor | ( | cv::InputArray | image, |
cv::OutputArray | binary, | ||
rm::CampType | enemy, | ||
int | lowerBound, | ||
bool | overExposed = false , |
||
cv::Size | kernelSize = {0, 0} |
||
) |
Extract specified color from source image.
image | Source image. |
binary | Output binary image. |
ownCamp | Own camp. |
overExposed | Specify if the image has been over exposed. If true, the color of own camp would be extracted somehow, consider pass source image to rm::FindLightBlobs function when finding light blobs. |
lowerBound | Lower bound when performing binarization. |
kernelSize | Kernel size when performing dilate. |
void rm::FindArmour | ( | std::vector< rm::LightBlob > & | lightBlobs, |
std::vector< rm::Armour > & | armours, | ||
float | maxAngleDif, | ||
float | errAngle, | ||
float | minBoxRatio, | ||
float | maxBoxRatio, | ||
float | lenRatio, | ||
rm::CampType | enemy, | ||
bool | filter = true |
||
) |
Fit armours from a set of light blobs.
lightBlobs | The set of light blobs |
armours | Output armours |
maxAngleDif | Maximum angle difference between two light blobs. |
errAngle | Maximum angle between the over all rect and the two light blobs. |
minBoxRatio | Minimum ratio of the armour box. |
maxBoxRatio | Maximum ratio of the armour box. |
lenRatio | Maximum length ratio between two light blobs. |
ownCamp | Own camp. |
attention | Attention point. When this parameter is specified, the distance between armour and the attention point would be used to sort the output. |
filter | Exclude one if two armours sharing a same light blob, exclude the armour witch minimum height along two light blobs is smaller. |
int rm::FindGuidLight | ( | const std::vector< rm::LightBlob > & | lightBlobs, |
const cv::Mat & | source | ||
) |
Find guid light
lightBlobs | |
source |
void rm::FindLightBlobs | ( | std::vector< rm::Contour > & | contours, |
std::vector< rm::LightBlob > & | lightBlobs, | ||
float | minRatio, | ||
float | maxRatio, | ||
float | tiltAngle, | ||
float | minArea, | ||
float | maxArea, | ||
const cv::Mat & | source, | ||
bool | fitEllipse = true |
||
) |
Find light blobs from a set of contours. Auto detect light blob camp.
contours | Input contour (more than 6 points). |
lightBlobs | Output light blobs. |
minRatio | Minimal aspect ratio. |
maxRatio | Maximal aspect ratio. |
tiltAngle | Maximal tilt angle. |
minArea | Minimal contour area. |
maxArea | Maximal contour area. |
source | Source image (3 channels RGB). |
fitEllipse | Use cv::fitEllipseDirect to box light blob instead of cv::minAreaRect when True. |
void rm::FindLightBlobs | ( | std::vector< rm::Contour > & | contours, |
std::vector< rm::LightBlob > & | lightBlobs, | ||
float | minRatio, | ||
float | maxRatio, | ||
float | tiltAngle, | ||
float | minArea, | ||
float | maxArea, | ||
rm::CampType | camp, | ||
bool | fitEllipse = true |
||
) |
Find light blobs from a set of contours.
contours | Input contour (more than 6 points). |
lightBlobs | Output light blobs. |
minRatio | Minimal aspect ratio. |
maxRatio | Maximal aspect ratio. |
tiltAngle | Maximal tilt angle. |
minArea | Minimal contour area. |
maxArea | Maximal contour area. |
camp | Light blobs camp. |
fitEllipse | Use cv::fitEllipseDirect to box light blob instead of cv::minAreaRect when True. |
bool rm::LightBlobOverlap | ( | std::vector< rm::LightBlob > & | lightBlobs, |
int | leftIndex, | ||
int | rightIndex | ||
) |
Detect if there is a overlap over all light blobs at the given pair of light blobs.
lightBlobs | All light blobs on the frame (must be sorted ascending by x). |
leftIndex | Index of the left light blob. |
rightIndex | Index of the right light blob. |
cv::Point2f rm::LineCenter | ( | const cv::Point2f & | pt1, |
const cv::Point2f & | pt2 | ||
) |
Return the midpoint between two given points.
pt1 | First point. |
pt2 | Second point. |
bool rm::MatchLightBlob | ( | const rm::Contour & | contour, |
float | minRatio, | ||
float | maxRatio, | ||
float | tiltAngle, | ||
float | minArea, | ||
float | maxArea, | ||
cv::RotatedRect & | lightBlobBox, | ||
bool | fitEllipse = true |
||
) |
Match a contour if it's a light blob with the given condition.
contour | Input contour (more than 6 points). |
minRatio | Minimal aspect ratio. |
maxRatio | Maximal aspect ratio. |
tiltAngle | Maximal tilt angle. |
minArea | Minimal contour area. |
maxArea | Maximal contour area. |
fitEllipse | Use cv::fitEllipseDirect to define light blob outlines instead of cv::minAreaRect when set to true. Usually recommended when source frame was over exposed. |
double rm::NewtonIteration | ( | double(*)(double) | fd, |
double | x0 = 0 , |
||
double | error = 0.0001 , |
||
int | cycle = 1024 |
||
) |
Use newton's iteration to approach the approx solve of function.
fd | The f(x)/f'(x) function of goal function. |
x0 | x to start the iteration. |
error | Maximum error before iteration stops. |
cycle | Maximum cycle the iteration could run. |
double rm::NewtonIteration | ( | double(*)(double, std::vector< double >) | fd, |
const std::vector< double > & | literals, | ||
double | x0 = 0 , |
||
double | error = 0.0001 , |
||
int | cycle = 1024 |
||
) |
Use newton's iteration to approach the approx solve of function.
fd | The f(x)/f'(x) function of goal function. |
literals | The literals that all f(x) needed. |
x0 | x to start the iteration. |
error | Maximum error before iteration stops. |
cycle | Maximum cycle the iteration could run. |
float rm::PointDistance | ( | const cv::Point2f & | pt1, |
const cv::Point2f & | pt2 | ||
) |
Return the distance between two given points.
pt1 | First point. |
pt2 | Second point. |
float rm::PointDistance | ( | const cv::Point2i & | pt1, |
const cv::Point2i & | pt2 | ||
) |
Return the distance between two given points.
pt1 | First point. |
pt2 | Second point. |
double rm::ProjectileAngle | ( | double | v0, |
double | g, | ||
double | d, | ||
double | h | ||
) |
Solve initial angle required for a projectile motion.
This function would be done by solving \( 0 = \frac{g \cdot d^{2}}{2 \cdot v_{0}^{2}} \cdot tan\theta^{2} + d \cdot tan\theta + (\frac{g \cdot d^{2}}{2 \cdot v_{0}^{2}} - h) \) for \(tan\theta\) using the binary first-order root formula, witch can be deduced from Newtonian's theorem of mechanics.
v0 | Initial launch speed. (m/s) |
g | Acceleration of gravity. (m/s^2) |
d | Horizontal distance. (m) |
h | Height difference. (m) |
double rm::ProjectileMotionFD | ( | double | theta, |
std::vector< double > | literals | ||
) |
The f(x)/f'(x) function of projectile motion.
theta | The angle of the initial shot of the oblique throwing motion, in radians. |
literals | The independent variables, in the order of g, d, h, v0. |
bool rm::SolveCameraPose | ( | cv::InputArray | rotationVector, |
cv::InputArray | translationVector, | ||
cv::OutputArray | pose | ||
) |
Solve camera pose in pitch, yaw, and roll relative to a target.
rotationVector | The rotation vector of target. |
translationVector | The translation vector of target. |
pose | [OUT] Camera pose. |
double rm::SolveGEA | ( | cv::InputArray | translationVector, |
cv::OutputArray | gimbalErrorAngle, | ||
double | g, | ||
double | v0, | ||
double | h, | ||
const cv::Point2f & | offset = {0, 0} , |
||
double | angleOffset = 0 , |
||
rm::CompensateMode | mode = rm::COMPENSATE_NONE |
||
) |
Solve gimbal error angle to target by given method.
translationVector | Translation vector of target. |
gimbalErrorAngle | [OUT] Estimation error angle. Format: [pitch, yaw]. |
g | Acceleration of gravity. (m/s^2) |
v0 | The initial speed of bullet. (m/s) |
h | Height between barrel and target. (m) |
offset | Offset between camera and barrel. (cm) |
angleOffset | Angle offset between camera and barrel. (RAD) |
mode | Method to be used to calculate the compensation of gravity. |
bool rm::SolvePNP | ( | const std::vector< cv::Point2f > & | imagePoints, |
cv::InputArray | cameraMatrix, | ||
cv::InputArray | distortionFactor, | ||
const cv::Size2f & | exactSize, | ||
cv::OutputArray | translationVector, | ||
cv::OutputArray | rotationVector, | ||
const cv::Rect & | ROI = {0, 0, 0, 0} |
||
) |
Solve the rotation & translation vector using cv::solvePnP & cv::SOLVEPNP_IPPE_SQUARE.
imagePoints | Object points on image (Quantity must be four). |
cameraMatrix | Camera matrix. |
distortionFactor | Camera distortion factor. |
exactSize | Exact size of the coordinate object (cm). |
translationVector | [OUT] Translation vector. |
rotationVector | [OUT] Rotation vector. |
void rm::VerticesRectify | ( | cv::RotatedRect & | input, |
cv::Point2f * | output, | ||
RectType | type = RECT_TALL |
||
) |
Reorder vertices of a rectangle to match the point order in left down, left up, right up, right down.
input | The original rectangle. |
output | Reordered vertices. |
type | Rectangle type. |
const unsigned char rm::CRC8[256] |