Get a single line representation for multiple close by lines clustered together in opencv - c++

I detected lines in an image and drew them in a separate image file in OpenCv C++ using HoughLinesP method. Following is a part of that resulting image. There are actually hundreds of small and thin lines which form a big single line.
But I want single few lines that represent all those number of lines. Closer lines should be merged together to form a single line. For example above set of lines should be represented by just 3 separate lines as below.
The expected output is as above. How to accomplish this task.
Up to now progress result from akarsakov's answer.
(separate classes of lines resulted are drawn in different colors). Note that this result is the original complete image I am working on, but not the sample section I had used in the question

If you don't know the number of lines in the image you can use the cv::partition function to split lines on equivalency group.
I suggest you the following procedure:
Split your lines using cv::partition. You need to specify a good predicate function. It really depends on lines which you extract from image, but I think it should check following conditions:
Angle between lines should be quite small (less 3 degrees, for example). Use dot product to calculate angle's cosine.
Distance between centers of segments should be less than half of maximum length of two segments.
For example, it can be implemented as follows:
bool isEqual(const Vec4i& _l1, const Vec4i& _l2)
{
Vec4i l1(_l1), l2(_l2);
float length1 = sqrtf((l1[2] - l1[0])*(l1[2] - l1[0]) + (l1[3] - l1[1])*(l1[3] - l1[1]));
float length2 = sqrtf((l2[2] - l2[0])*(l2[2] - l2[0]) + (l2[3] - l2[1])*(l2[3] - l2[1]));
float product = (l1[2] - l1[0])*(l2[2] - l2[0]) + (l1[3] - l1[1])*(l2[3] - l2[1]);
if (fabs(product / (length1 * length2)) < cos(CV_PI / 30))
return false;
float mx1 = (l1[0] + l1[2]) * 0.5f;
float mx2 = (l2[0] + l2[2]) * 0.5f;
float my1 = (l1[1] + l1[3]) * 0.5f;
float my2 = (l2[1] + l2[3]) * 0.5f;
float dist = sqrtf((mx1 - mx2)*(mx1 - mx2) + (my1 - my2)*(my1 - my2));
if (dist > std::max(length1, length2) * 0.5f)
return false;
return true;
}
Guess you have your lines in vector<Vec4i> lines;. Next, you should call cv::partition as follows:
vector<Vec4i> lines;
std::vector<int> labels;
int numberOfLines = cv::partition(lines, labels, isEqual);
You need to call cv::partition once and it will clusterize all lines. Vector labels will store for each line label of cluster to which it belongs. See documentation for cv::partition
After you get all groups of line you should merge them. I suggest calculating average angle of all lines in group and estimate "border" points. For example, if angle is zero (i.e. all lines are almost horizontal) it would be the left-most and right-most points. It remains only to draw a line between this points.
I noticed that all lines in your examples are horizontal or vertical. In such case you can calculate point which is average of all segment's centers and "border" points, and then just draw horizontal or vertical line limited by "border" points through center point.
Please note that cv::partition takes O(N^2) time, so if you process a huge number of lines it may take a lot of time.
I hope it will help. I used such approach for similar task.

First off I want to note that your original image is at a slight angle, so your expected output seems just a bit off to me. I'm assuming you are okay with lines that are not 100% vertical in your output because they are slightly off on your input.
Mat image;
Mat binary = image > 125; // Convert to binary image
// Combine similar lines
int size = 3;
Mat element = getStructuringElement( MORPH_ELLIPSE, Size( 2*size + 1, 2*size+1 ), Point( size, size ) );
morphologyEx( mask, mask, MORPH_CLOSE, element );
So far this yields this image:
These lines are not at 90 degree angles because the original image is not.
You can also choose to close the gap between the lines with:
Mat out = Mat::zeros(mask.size(), mask.type());
vector<Vec4i> lines;
HoughLinesP(mask, lines, 1, CV_PI/2, 50, 50, 75);
for( size_t i = 0; i < lines.size(); i++ )
{
Vec4i l = lines[i];
line( out, Point(l[0], l[1]), Point(l[2], l[3]), Scalar(255), 5, CV_AA);
}
If these lines are too fat, I've had success thinning them with:
size = 15;
Mat eroded;
cv::Mat erodeElement = getStructuringElement( MORPH_ELLIPSE, cv::Size( size, size ) );
erode( mask, eroded, erodeElement );

Here is a refinement build upon #akarsakov answer.
A basic issue with:
Distance between centers of segments should be less than half of
maximum length of two segments.
is that parallel long lines that are visually far might end up in same equivalence class (as demonstrated in OP's edit).
Therefore the approach that I found working reasonable for me:
Construct a window (bounding rectangle) around a line1.
line2 angle is close enough to line1's and at least one point of line2 is inside line1's bounding rectangle
Often a long linear feature in the image that is quite weak will end up recognized (HoughP, LSD) by a set of line segments with considerable gaps between them. To alleviate this, our bounding rectangle is constructed around line extended in both directions, where extension is defined by a fraction of original line width.
bool extendedBoundingRectangleLineEquivalence(const Vec4i& _l1, const Vec4i& _l2, float extensionLengthFraction, float maxAngleDiff, float boundingRectangleThickness){
Vec4i l1(_l1), l2(_l2);
// extend lines by percentage of line width
float len1 = sqrtf((l1[2] - l1[0])*(l1[2] - l1[0]) + (l1[3] - l1[1])*(l1[3] - l1[1]));
float len2 = sqrtf((l2[2] - l2[0])*(l2[2] - l2[0]) + (l2[3] - l2[1])*(l2[3] - l2[1]));
Vec4i el1 = extendedLine(l1, len1 * extensionLengthFraction);
Vec4i el2 = extendedLine(l2, len2 * extensionLengthFraction);
// reject the lines that have wide difference in angles
float a1 = atan(linearParameters(el1)[0]);
float a2 = atan(linearParameters(el2)[0]);
if(fabs(a1 - a2) > maxAngleDiff * M_PI / 180.0){
return false;
}
// calculate window around extended line
// at least one point needs to inside extended bounding rectangle of other line,
std::vector<Point2i> lineBoundingContour = boundingRectangleContour(el1, boundingRectangleThickness/2);
return
pointPolygonTest(lineBoundingContour, cv::Point(el2[0], el2[1]), false) == 1 ||
pointPolygonTest(lineBoundingContour, cv::Point(el2[2], el2[3]), false) == 1;
}
where linearParameters, extendedLine, boundingRectangleContour are following:
Vec2d linearParameters(Vec4i line){
Mat a = (Mat_<double>(2, 2) <<
line[0], 1,
line[2], 1);
Mat y = (Mat_<double>(2, 1) <<
line[1],
line[3]);
Vec2d mc; solve(a, y, mc);
return mc;
}
Vec4i extendedLine(Vec4i line, double d){
// oriented left-t-right
Vec4d _line = line[2] - line[0] < 0 ? Vec4d(line[2], line[3], line[0], line[1]) : Vec4d(line[0], line[1], line[2], line[3]);
double m = linearParameters(_line)[0];
// solution of pythagorean theorem and m = yd/xd
double xd = sqrt(d * d / (m * m + 1));
double yd = xd * m;
return Vec4d(_line[0] - xd, _line[1] - yd , _line[2] + xd, _line[3] + yd);
}
std::vector<Point2i> boundingRectangleContour(Vec4i line, float d){
// finds coordinates of perpendicular lines with length d in both line points
// https://math.stackexchange.com/a/2043065/183923
Vec2f mc = linearParameters(line);
float m = mc[0];
float factor = sqrtf(
(d * d) / (1 + (1 / (m * m)))
);
float x3, y3, x4, y4, x5, y5, x6, y6;
// special case(vertical perpendicular line) when -1/m -> -infinity
if(m == 0){
x3 = line[0]; y3 = line[1] + d;
x4 = line[0]; y4 = line[1] - d;
x5 = line[2]; y5 = line[3] + d;
x6 = line[2]; y6 = line[3] - d;
} else {
// slope of perpendicular lines
float m_per = - 1/m;
// y1 = m_per * x1 + c_per
float c_per1 = line[1] - m_per * line[0];
float c_per2 = line[3] - m_per * line[2];
// coordinates of perpendicular lines
x3 = line[0] + factor; y3 = m_per * x3 + c_per1;
x4 = line[0] - factor; y4 = m_per * x4 + c_per1;
x5 = line[2] + factor; y5 = m_per * x5 + c_per2;
x6 = line[2] - factor; y6 = m_per * x6 + c_per2;
}
return std::vector<Point2i> {
Point2i(x3, y3),
Point2i(x4, y4),
Point2i(x6, y6),
Point2i(x5, y5)
};
}
To partion, call:
std::vector<int> labels;
int equilavenceClassesCount = cv::partition(linesWithoutSmall, labels, [](const Vec4i l1, const Vec4i l2){
return extendedBoundingRectangleLineEquivalence(
l1, l2,
// line extension length - as fraction of original line width
0.2,
// maximum allowed angle difference for lines to be considered in same equivalence class
2.0,
// thickness of bounding rectangle around each line
10);
});
Now, in order to reduce each equivalence class to single line, we build a point cloud out of it and find a line fit:
// fit line to each equivalence class point cloud
std::vector<Vec4i> reducedLines = std::accumulate(pointClouds.begin(), pointClouds.end(), std::vector<Vec4i>{}, [](std::vector<Vec4i> target, const std::vector<Point2i>& _pointCloud){
std::vector<Point2i> pointCloud = _pointCloud;
//lineParams: [vx,vy, x0,y0]: (normalized vector, point on our contour)
// (x,y) = (x0,y0) + t*(vx,vy), t -> (-inf; inf)
Vec4f lineParams; fitLine(pointCloud, lineParams, CV_DIST_L2, 0, 0.01, 0.01);
// derive the bounding xs of point cloud
decltype(pointCloud)::iterator minXP, maxXP;
std::tie(minXP, maxXP) = std::minmax_element(pointCloud.begin(), pointCloud.end(), [](const Point2i& p1, const Point2i& p2){ return p1.x < p2.x; });
// derive y coords of fitted line
float m = lineParams[1] / lineParams[0];
int y1 = ((minXP->x - lineParams[2]) * m) + lineParams[3];
int y2 = ((maxXP->x - lineParams[2]) * m) + lineParams[3];
target.push_back(Vec4i(minXP->x, y1, maxXP->x, y2));
return target;
});
Demonstration:
Detected partitioned line (with small lines filtered out):
Reduced:
Demonstration code:
int main(int argc, const char* argv[]){
if(argc < 2){
std::cout << "img filepath should be present in args" << std::endl;
}
Mat image = imread(argv[1]);
Mat smallerImage; resize(image, smallerImage, cv::Size(), 0.5, 0.5, INTER_CUBIC);
Mat target = smallerImage.clone();
namedWindow("Detected Lines", WINDOW_NORMAL);
namedWindow("Reduced Lines", WINDOW_NORMAL);
Mat detectedLinesImg = Mat::zeros(target.rows, target.cols, CV_8UC3);
Mat reducedLinesImg = detectedLinesImg.clone();
// delect lines in any reasonable way
Mat grayscale; cvtColor(target, grayscale, CV_BGRA2GRAY);
Ptr<LineSegmentDetector> detector = createLineSegmentDetector(LSD_REFINE_NONE);
std::vector<Vec4i> lines; detector->detect(grayscale, lines);
// remove small lines
std::vector<Vec4i> linesWithoutSmall;
std::copy_if (lines.begin(), lines.end(), std::back_inserter(linesWithoutSmall), [](Vec4f line){
float length = sqrtf((line[2] - line[0]) * (line[2] - line[0])
+ (line[3] - line[1]) * (line[3] - line[1]));
return length > 30;
});
std::cout << "Detected: " << linesWithoutSmall.size() << std::endl;
// partition via our partitioning function
std::vector<int> labels;
int equilavenceClassesCount = cv::partition(linesWithoutSmall, labels, [](const Vec4i l1, const Vec4i l2){
return extendedBoundingRectangleLineEquivalence(
l1, l2,
// line extension length - as fraction of original line width
0.2,
// maximum allowed angle difference for lines to be considered in same equivalence class
2.0,
// thickness of bounding rectangle around each line
10);
});
std::cout << "Equivalence classes: " << equilavenceClassesCount << std::endl;
// grab a random colour for each equivalence class
RNG rng(215526);
std::vector<Scalar> colors(equilavenceClassesCount);
for (int i = 0; i < equilavenceClassesCount; i++){
colors[i] = Scalar(rng.uniform(30,255), rng.uniform(30, 255), rng.uniform(30, 255));;
}
// draw original detected lines
for (int i = 0; i < linesWithoutSmall.size(); i++){
Vec4i& detectedLine = linesWithoutSmall[i];
line(detectedLinesImg,
cv::Point(detectedLine[0], detectedLine[1]),
cv::Point(detectedLine[2], detectedLine[3]), colors[labels[i]], 2);
}
// build point clouds out of each equivalence classes
std::vector<std::vector<Point2i>> pointClouds(equilavenceClassesCount);
for (int i = 0; i < linesWithoutSmall.size(); i++){
Vec4i& detectedLine = linesWithoutSmall[i];
pointClouds[labels[i]].push_back(Point2i(detectedLine[0], detectedLine[1]));
pointClouds[labels[i]].push_back(Point2i(detectedLine[2], detectedLine[3]));
}
// fit line to each equivalence class point cloud
std::vector<Vec4i> reducedLines = std::accumulate(pointClouds.begin(), pointClouds.end(), std::vector<Vec4i>{}, [](std::vector<Vec4i> target, const std::vector<Point2i>& _pointCloud){
std::vector<Point2i> pointCloud = _pointCloud;
//lineParams: [vx,vy, x0,y0]: (normalized vector, point on our contour)
// (x,y) = (x0,y0) + t*(vx,vy), t -> (-inf; inf)
Vec4f lineParams; fitLine(pointCloud, lineParams, CV_DIST_L2, 0, 0.01, 0.01);
// derive the bounding xs of point cloud
decltype(pointCloud)::iterator minXP, maxXP;
std::tie(minXP, maxXP) = std::minmax_element(pointCloud.begin(), pointCloud.end(), [](const Point2i& p1, const Point2i& p2){ return p1.x < p2.x; });
// derive y coords of fitted line
float m = lineParams[1] / lineParams[0];
int y1 = ((minXP->x - lineParams[2]) * m) + lineParams[3];
int y2 = ((maxXP->x - lineParams[2]) * m) + lineParams[3];
target.push_back(Vec4i(minXP->x, y1, maxXP->x, y2));
return target;
});
for(Vec4i reduced: reducedLines){
line(reducedLinesImg, Point(reduced[0], reduced[1]), Point(reduced[2], reduced[3]), Scalar(255, 255, 255), 2);
}
imshow("Detected Lines", detectedLinesImg);
imshow("Reduced Lines", reducedLinesImg);
waitKey();
return 0;
}

I would recommend that you use HoughLines from OpenCV.
void HoughLines(InputArray image, OutputArray lines, double rho, double theta, int threshold, double srn=0, double stn=0 )
You can adjust with rho and theta the possible orientation and position of the lines you want to observe.
In your case, theta = 90° would be fine (only vertical and horizontal lines).
After this, you can get unique line equations with Plücker coordinates. And from there you could apply a K-mean with 3 centers that should fit approximately your 3 lines in the second image.
PS : I will see if i can test the whole process with your image

You can merge multiple close line into single line by clustering lines using rho and theta and finally taking average of rho and theta.
void contourLines(vector<cv::Vec2f> lines, const float rho_threshold, const float theta_threshold, vector< cv::Vec2f > &combinedLines)
{
vector< vector<int> > combineIndex(lines.size());
for (int i = 0; i < lines.size(); i++)
{
int index = i;
for (int j = i; j < lines.size(); j++)
{
float distanceI = lines[i][0], distanceJ = lines[j][0];
float slopeI = lines[i][1], slopeJ = lines[j][1];
float disDiff = abs(distanceI - distanceJ);
float slopeDiff = abs(slopeI - slopeJ);
if (slopeDiff < theta_max && disDiff < rho_max)
{
bool isCombined = false;
for (int w = 0; w < i; w++)
{
for (int u = 0; u < combineIndex[w].size(); u++)
{
if (combineIndex[w][u] == j)
{
isCombined = true;
break;
}
if (combineIndex[w][u] == i)
index = w;
}
if (isCombined)
break;
}
if (!isCombined)
combineIndex[index].push_back(j);
}
}
}
for (int i = 0; i < combineIndex.size(); i++)
{
if (combineIndex[i].size() == 0)
continue;
cv::Vec2f line_temp(0, 0);
for (int j = 0; j < combineIndex[i].size(); j++) {
line_temp[0] += lines[combineIndex[i][j]][0];
line_temp[1] += lines[combineIndex[i][j]][1];
}
line_temp[0] /= combineIndex[i].size();
line_temp[1] /= combineIndex[i].size();
combinedLines.push_back(line_temp);
}
}
function call
You can tune houghThreshold, rho_threshold and theta_threshold as per your application.
HoughLines(edge, lines_t, 1, CV_PI / 180, houghThreshold, 0, 0);
float rho_threshold= 15;
float theta_threshold = 3*DEGREES_TO_RADIANS;
vector< cv::Vec2f > lines;
contourCluster(lines_t, rho_max, theta_max, lines);

#C_Raj made a good point, for lines like this, i.e., most likely extracted from table/form-like images, you should make full use of the fact that many of the line segments captured by Hough transform from the same lines have very similar \rho and \theta.
After clustering these line segments based on their \rho and \theta, you can apply 2D line fitting to obtain estimate of the true lines in an image.
There is a paper describing this idea and it's making further assumptions of the lines in a page.
HTH.

Related

weird inaccuracy in line rotation - c++

I have programmed a simple dragon curve fractal. It seems to work for the most part, but there is an odd logical error that shifts the rotation of certain lines by one pixel. This wouldn't normally be an issue, but after a few generations, at the right size, the fractal begins to look wonky.
I am using open cv in c++ to generate it, but I'm pretty sure it's a logical error rather than a display error. I have printed the values to the console multiple times and seen for myself that there is a one-digit difference between values that are intended to be the exact same - meaning a line may have a y of 200 at one end and 201 at another.
Here is the full code:
#include<iostream>
#include<cmath>
#include<opencv2/opencv.hpp>
const int width=500;
const int height=500;
const double PI=std::atan(1)*4.0;
struct point{
double x;
double y;
point(double x_,double y_){
x=x_;
y=y_;
}};
cv::Mat img(width,height,CV_8UC3,cv::Scalar(255,255,255));
double deg_to_rad(double degrees){return degrees*PI/180;}
point rotate(int degree, int centx, int centy, int ll) {
double radians = deg_to_rad(degree);
return point(centx + (ll * std::cos(radians)), centy + (ll * std::sin(radians)));
}
void generate(point & r, std::vector < point > & verticies, int rotation = 90) {
int curRotation = 90;
bool start = true;
point center = r;
point rot(0, 0);
std::vector<point> verticiesc(verticies);
for (point i: verticiesc) {
double dx = center.x - i.x;
double dy = center.y - i.y;
//distance from centre
int ll = std::sqrt(dx * dx + dy * dy);
//angle from centre
curRotation = std::atan2(dy, dx) * 180 / PI;
//add 90 degrees of rotation
rot = rotate(curRotation + rotation, center.x, center.y, ll);
verticies.push_back(rot);
//endpoint, where the next centre will be
if (start) {
r = rot;
start = false;
}
}
}
void gen(int gens, int bwidth = 1) {
int ll = 7;
std::vector < point > verticies = {
point(width / 2, height / 2 - ll),
point(width / 2, height / 2)
};
point rot(width / 2, height / 2);
for (int i = 0; i < gens; i++) {
generate(rot, verticies);
}
//draw lines
for (int i = 0; i < verticies.size(); i += 2) {
cv::line(img, cv::Point(verticies[i].x, verticies[i].y), cv::Point(verticies[i + 1].x, verticies[i + 1].y), cv::Scalar(0, 0, 0), 1, 8);
}
}
int main() {
gen(10);
cv::imshow("", img);
cv::waitKey(0);
return 0;
}
First, you use int to store point coordinates - that's a bad idea - you lose all accuracy of point position. Use double or float.
Second, your method for drawing fractals is not too stable numericly. You'd better store original shape and all rotation/translation/scale that indicate where and how to draw scaled copies of the original shape.
Also, I believe this is a bug:
for(point i: verices)
{
...
vertices.push_back(rot);
...
}
Changing size of vertices while inside such a for-loop might cause a crash or UB.
Turns out it was to do with floating-point precision. I changed
x=x_;
y=y_;
to
x=std::round(x_);
y=std::round(y_);
and it works.

Find intersection points for vector construct

So in my software I have two vectors. The first vector matrix stores the information of the shape of a given 3D model. So I got a vector of arrays to store the x,y,z coordinates of points.
std::vector<std::array<double, 3>> matrix;
This vector is already sorted, so that I get the contour of the model.
In the second vector boundingbox I store the information of a bounding box.
std::vector<std::array<double, 3>> boundingbox;
In this vector the first four elements describe the bounding box around the contour. To fill the outline I have placed a grid on it. The grid is in this case defined by the software based on a variable. The variable infill is set by the user at run-time. So currently my program creats the following image.
Now the next step would be to find the intersection points between the grid and the contour. My approach to this would be a typical mathematical approach.
I would use two for-loops. The first loop would be used to iterate over the grid so that each line of the grid is called once.
The second loop would be used the vector to undergo matrix. I developed a pseudo code, in which I describe my procedure.
int fillingStart; //first element of boundingbox to contain information about the grid
int n; //number of lines in the Grid.
for(size_t i=fillingStart; i<(n-1); i+2)
{
double A_x=boundingbox[j][0];
double A_y=boundingbox[j][1];
double B_x=boundingbox[j+1][0];
double B_y=boundingbox[j+1][0];
double AB_x=B_x-A_x;
double AB_y=B_y-A_y;
double intersectionpoint_y = DBL_MAX;
double intersectionpoint_x = DBL_MAX;
double intersectionpoint2_y = DBL_MAX;
double intersectionpoint2_x = DBL_MAX;
for(size_t j=0; j<(matrix.size()-1); j++)
{
double C_x = matrix[j][0];
double C_y = matrix[j][1];
double D_x = matrix[j+1][0];
double D_y = matrix[j+1][1];
double CD_x = D_x-C_x;
double CD_y = D_y-C_y;
double s = (((C_x-A_x)*(-CD_y))-((-CD_x)*(C_y-A_y)))/((AB_x*(-CD_y))-((-CD_x)*AB_y));//Cramer's rule
double t = ((AB_x*(C_y-A_y))-((C_x-A_x)*AB_y)) / ((AB_x * (-CD_y))-((-CD_x)*AB_y));//Cramer's rule
double point_x = A_x+s*AB_x;
double point_y = A_y*s*AB_y;
if(point_x < intersectionpoint_x && point_y < intersectionpoint_y)
{
intersectionpoint_x = point_x;
intersectionpoint_y = point_y;
}
else if(point_x < intersectionpoint2_x && point_y < intersectionpoint2_y)
{
intersectionpoint2_x = point_x;
intersectionpoint2_y = point_y;
}
}
intersects.push_back(std::array<double, 3>());
double q = boundingbox.size()-1;
intersects[q][0] = intersectionpoint_x;
intersects[q][1] = intersectionpoint_y;
intersects.push_back(std::array<double, 3>());
double q = boundingbox.size()-1;
intersects[q][0] = intersectionpoint2_x;
intersects[q][1] = intersectionpoint2_y;
}
With this two loops I would find the intersection points for each line of the grid and each vector (between two points) of the contour. Then I would have to find the two intersection points, closest to the grid line and store these points. The special case would be, if there is something in the contoure, like a hole. In this case I would find four points.
EDIT: Why I want to use intersection points is shown in the following figures
Here we have the contour of a rectangle. As you can see there are just a few points to describe the figure.
The next image shows the filling of the model
Because of the few points of the contour I have to calculate the intersection points of the contour and the grid.
EDIT2: I now got the code working and updated the code here, but the problem is that it saves always the same point in intersectionpoint. Thats because of the if-statement, but I cant figure out how get it working.
You could iterate over the contour, and for each two consecutive points, check if there is a line between, and if there is one, compute the intersection point.
std::vector<std::array<double, 3>> intersects;
auto it = matrix.begin();
while (it != matrix.end() - 1) {
auto &p1 = *it;
auto &p2 = *(++it);
double x;
// Check if there is a vertical line between p1 and p2
if (has_vertical_line(p1, p2, &x)) {
// The equation of the line joining p1 and p2 is:
// (p2[1] - p1[1]) / (p2[0] - p1[0]) * x + p1[0]
double y = (p2[1] - p1[1]) / (p2[0] - p1[0]) * x + p1[0];
intersects.push_back({x, y, 0.0});
}
}
Where has_vertical_line is something like:
bool has_vertical_line (std::array<double, 3> const& p1,
std::array<double, 3> const& p2,
double *px) {
double x1 = p1[0], x2 = p2[0];
if (x2 <= x1) {
std::swap(x1, x2);
}
size_t lx2 = closest_from_below(x2),
lx1 = closest_from_above(x1);
if (lx1 == lx2) {
*px = lines[lx1]; // Assuming abscissa
return true;
}
return false;
}
Where closest_from_below and closest_from_above are simple function that find the line just below / above the current abscissa (trivial since your lines are vertical).

Rotate an image without cropping in OpenCV in C++

I'd like to rotate an image, but I can't obtain the rotated image without cropping
My original image:
Now I use this code:
#include <opencv2/core/core.hpp>
#include <opencv2/highgui/highgui.hpp>
#include <opencv2/imgproc/imgproc.hpp>
// Compile with g++ code.cpp -lopencv_core -lopencv_highgui -lopencv_imgproc
int main()
{
cv::Mat src = cv::imread("im.png", CV_LOAD_IMAGE_UNCHANGED);
cv::Mat dst;
cv::Point2f pc(src.cols/2., src.rows/2.);
cv::Mat r = cv::getRotationMatrix2D(pc, -45, 1.0);
cv::warpAffine(src, dst, r, src.size()); // what size I should use?
cv::imwrite("rotated_im.png", dst);
return 0;
}
And obtain the following image:
But I'd like to obtain this:
My answer is inspired by the following posts / blog entries:
Rotate cv::Mat using cv::warpAffine offsets destination image
http://john.freml.in/opencv-rotation
Main ideas:
Adjusting the rotation matrix by adding a translation to the new image center
Using cv::RotatedRect to rely on existing opencv functionality as much as possible
Code tested with opencv 3.4.1:
#include "opencv2/opencv.hpp"
int main()
{
cv::Mat src = cv::imread("im.png", CV_LOAD_IMAGE_UNCHANGED);
double angle = -45;
// get rotation matrix for rotating the image around its center in pixel coordinates
cv::Point2f center((src.cols-1)/2.0, (src.rows-1)/2.0);
cv::Mat rot = cv::getRotationMatrix2D(center, angle, 1.0);
// determine bounding rectangle, center not relevant
cv::Rect2f bbox = cv::RotatedRect(cv::Point2f(), src.size(), angle).boundingRect2f();
// adjust transformation matrix
rot.at<double>(0,2) += bbox.width/2.0 - src.cols/2.0;
rot.at<double>(1,2) += bbox.height/2.0 - src.rows/2.0;
cv::Mat dst;
cv::warpAffine(src, dst, rot, bbox.size());
cv::imwrite("rotated_im.png", dst);
return 0;
}
Just try the code below, the idea is simple:
You need to create a blank image with the maximum size you're expecting while rotating at any angle. Here you should use Pythagoras as mentioned in the above comments.
Now copy the source image to the newly created image and pass it to warpAffine. Here you should use the centre of newly created image for rotation.
After warpAffine if you need to crop exact image for this translate four corners of source image in enlarged image using rotation matrix as described here
Find minimum x and minimum y for top corner, and maximum x and maximum y for bottom corner from the above result to crop image.
This is the code:
int theta = 0;
Mat src,frame, frameRotated;
src = imread("rotate.png",1);
cout<<endl<<endl<<"Press '+' to rotate anti-clockwise and '-' for clockwise 's' to save" <<endl<<endl;
int diagonal = (int)sqrt(src.cols*src.cols+src.rows*src.rows);
int newWidth = diagonal;
int newHeight =diagonal;
int offsetX = (newWidth - src.cols) / 2;
int offsetY = (newHeight - src.rows) / 2;
Mat targetMat(newWidth, newHeight, src.type());
Point2f src_center(targetMat.cols/2.0F, targetMat.rows/2.0F);
while(1){
src.copyTo(frame);
double radians = theta * M_PI / 180.0;
double sin = abs(std::sin(radians));
double cos = abs(std::cos(radians));
frame.copyTo(targetMat.rowRange(offsetY, offsetY + frame.rows).colRange(offsetX, offsetX + frame.cols));
Mat rot_mat = getRotationMatrix2D(src_center, theta, 1.0);
warpAffine(targetMat, frameRotated, rot_mat, targetMat.size());
//Calculate bounding rect and for exact image
//Reference:- https://stackoverflow.com/questions/19830477/find-the-bounding-rectangle-of-rotated-rectangle/19830964?noredirect=1#19830964
Rect bound_Rect(frame.cols,frame.rows,0,0);
int x1 = offsetX;
int x2 = offsetX+frame.cols;
int x3 = offsetX;
int x4 = offsetX+frame.cols;
int y1 = offsetY;
int y2 = offsetY;
int y3 = offsetY+frame.rows;
int y4 = offsetY+frame.rows;
Mat co_Ordinate = (Mat_<double>(3,4) << x1, x2, x3, x4,
y1, y2, y3, y4,
1, 1, 1, 1 );
Mat RotCo_Ordinate = rot_mat * co_Ordinate;
for(int i=0;i<4;i++){
if(RotCo_Ordinate.at<double>(0,i)<bound_Rect.x)
bound_Rect.x=(int)RotCo_Ordinate.at<double>(0,i); //access smallest
if(RotCo_Ordinate.at<double>(1,i)<bound_Rect.y)
bound_Rect.y=RotCo_Ordinate.at<double>(1,i); //access smallest y
}
for(int i=0;i<4;i++){
if(RotCo_Ordinate.at<double>(0,i)>bound_Rect.width)
bound_Rect.width=(int)RotCo_Ordinate.at<double>(0,i); //access largest x
if(RotCo_Ordinate.at<double>(1,i)>bound_Rect.height)
bound_Rect.height=RotCo_Ordinate.at<double>(1,i); //access largest y
}
bound_Rect.width=bound_Rect.width-bound_Rect.x;
bound_Rect.height=bound_Rect.height-bound_Rect.y;
Mat cropedResult;
Mat ROI = frameRotated(bound_Rect);
ROI.copyTo(cropedResult);
imshow("Result", cropedResult);
imshow("frame", frame);
imshow("rotated frame", frameRotated);
char k=waitKey();
if(k=='+') theta+=10;
if(k=='-') theta-=10;
if(k=='s') imwrite("rotated.jpg",cropedResult);
if(k==27) break;
}
Cropped Image
Thanks Robula!
Actually, you do not need to compute sine and cosine twice.
import cv2
def rotate_image(mat, angle):
# angle in degrees
height, width = mat.shape[:2]
image_center = (width/2, height/2)
rotation_mat = cv2.getRotationMatrix2D(image_center, angle, 1.)
abs_cos = abs(rotation_mat[0,0])
abs_sin = abs(rotation_mat[0,1])
bound_w = int(height * abs_sin + width * abs_cos)
bound_h = int(height * abs_cos + width * abs_sin)
rotation_mat[0, 2] += bound_w/2 - image_center[0]
rotation_mat[1, 2] += bound_h/2 - image_center[1]
rotated_mat = cv2.warpAffine(mat, rotation_mat, (bound_w, bound_h))
return rotated_mat
Thanks #Haris! Here's the Python version:
def rotate_image(image, angle):
'''Rotate image "angle" degrees.
How it works:
- Creates a blank image that fits any rotation of the image. To achieve
this, set the height and width to be the image's diagonal.
- Copy the original image to the center of this blank image
- Rotate using warpAffine, using the newly created image's center
(the enlarged blank image center)
- Translate the four corners of the source image in the enlarged image
using homogenous multiplication of the rotation matrix.
- Crop the image according to these transformed corners
'''
diagonal = int(math.sqrt(pow(image.shape[0], 2) + pow(image.shape[1], 2)))
offset_x = (diagonal - image.shape[0])/2
offset_y = (diagonal - image.shape[1])/2
dst_image = np.zeros((diagonal, diagonal, 3), dtype='uint8')
image_center = (diagonal/2, diagonal/2)
R = cv2.getRotationMatrix2D(image_center, angle, 1.0)
dst_image[offset_x:(offset_x + image.shape[0]), \
offset_y:(offset_y + image.shape[1]), \
:] = image
dst_image = cv2.warpAffine(dst_image, R, (diagonal, diagonal), flags=cv2.INTER_LINEAR)
# Calculate the rotated bounding rect
x0 = offset_x
x1 = offset_x + image.shape[0]
x2 = offset_x
x3 = offset_x + image.shape[0]
y0 = offset_y
y1 = offset_y
y2 = offset_y + image.shape[1]
y3 = offset_y + image.shape[1]
corners = np.zeros((3,4))
corners[0,0] = x0
corners[0,1] = x1
corners[0,2] = x2
corners[0,3] = x3
corners[1,0] = y0
corners[1,1] = y1
corners[1,2] = y2
corners[1,3] = y3
corners[2:] = 1
c = np.dot(R, corners)
x = int(c[0,0])
y = int(c[1,0])
left = x
right = x
up = y
down = y
for i in range(4):
x = int(c[0,i])
y = int(c[1,i])
if (x < left): left = x
if (x > right): right = x
if (y < up): up = y
if (y > down): down = y
h = down - up
w = right - left
cropped = np.zeros((w, h, 3), dtype='uint8')
cropped[:, :, :] = dst_image[left:(left+w), up:(up+h), :]
return cropped
Increase the image canvas (equally from the center without changing the image size) so that it can fit the image after rotation, then apply warpAffine:
Mat img = imread ("/path/to/image", 1);
double offsetX, offsetY;
double angle = -45;
double width = img.size().width;
double height = img.size().height;
Point2d center = Point2d (width / 2, height / 2);
Rect bounds = RotatedRect (center, img.size(), angle).boundingRect();
Mat resized = Mat::zeros (bounds.size(), img.type());
offsetX = (bounds.width - width) / 2;
offsetY = (bounds.height - height) / 2;
Rect roi = Rect (offsetX, offsetY, width, height);
img.copyTo (resized (roi));
center += Point2d (offsetX, offsetY);
Mat M = getRotationMatrix2D (center, angle, 1.0);
warpAffine (resized, resized, M, resized.size());
After searching around for a clean and easy to understand solution and reading through the answers above trying to understand them, I eventually came up with a solution using trigonometry.
I hope this helps somebody :)
import cv2
import math
def rotate_image(mat, angle):
height, width = mat.shape[:2]
image_center = (width / 2, height / 2)
rotation_mat = cv2.getRotationMatrix2D(image_center, angle, 1)
radians = math.radians(angle)
sin = math.sin(radians)
cos = math.cos(radians)
bound_w = int((height * abs(sin)) + (width * abs(cos)))
bound_h = int((height * abs(cos)) + (width * abs(sin)))
rotation_mat[0, 2] += ((bound_w / 2) - image_center[0])
rotation_mat[1, 2] += ((bound_h / 2) - image_center[1])
rotated_mat = cv2.warpAffine(mat, rotation_mat, (bound_w, bound_h))
return rotated_mat
EDIT: Please refer to #Remi Cuingnet's answer below.
A python version of rotating an image and take control of the padded black coloured region you can use the scipy.ndimage.rotate. Here is an example:
from skimage import io
from scipy import ndimage
image = io.imread('https://www.pyimagesearch.com/wp-
content/uploads/2019/12/tensorflow2_install_ubuntu_header.jpg')
io.imshow(image)
plt.show()
rotated = ndimage.rotate(image, angle=234, mode='nearest')
rotated = cv2.resize(rotated, (image.shape[:2]))
# rotated = cv2.cvtColor(rotated, cv2.COLOR_BGR2RGB)
# cv2.imwrite('rotated.jpg', rotated)
io.imshow(rotated)
plt.show()
If you have a rotation and a scaling of the image:
#include "opencv2/opencv.hpp"
#include <functional>
#include <vector>
bool compareCoords(cv::Point2f p1, cv::Point2f p2, char coord)
{
assert(coord == 'x' || coord == 'y');
if (coord == 'x')
return p1.x < p2.x;
return p1.y < p2.y;
}
int main(int argc, char** argv)
{
cv::Mat image = cv::imread("lenna.png");
float angle = 45.0; // degrees
float scale = 0.5;
cv::Mat_<float> rot_mat = cv::getRotationMatrix2D( cv::Point2f( 0.0f, 0.0f ), angle, scale );
// Image corners
cv::Point2f pA = cv::Point2f(0.0f, 0.0f);
cv::Point2f pB = cv::Point2f(image.cols, 0.0f);
cv::Point2f pC = cv::Point2f(image.cols, image.rows);
cv::Point2f pD = cv::Point2f(0.0f, image.rows);
std::vector<cv::Point2f> pts = { pA, pB, pC, pD };
std::vector<cv::Point2f> ptsTransf;
cv::transform(pts, ptsTransf, rot_mat );
using namespace std::placeholders;
float minX = std::min_element(ptsTransf.begin(), ptsTransf.end(), std::bind(compareCoords, _1, _2, 'x'))->x;
float maxX = std::max_element(ptsTransf.begin(), ptsTransf.end(), std::bind(compareCoords, _1, _2, 'x'))->x;
float minY = std::min_element(ptsTransf.begin(), ptsTransf.end(), std::bind(compareCoords, _1, _2, 'y'))->y;
float maxY = std::max_element(ptsTransf.begin(), ptsTransf.end(), std::bind(compareCoords, _1, _2, 'y'))->y;
float newW = maxX - minX;
float newH = maxY - minY;
cv::Mat_<float> trans_mat = (cv::Mat_<float>(2,3) << 0, 0, -minX, 0, 0, -minY);
cv::Mat_<float> M = rot_mat + trans_mat;
cv::Mat warpedImage;
cv::warpAffine( image, warpedImage, M, cv::Size(newW, newH) );
cv::imshow("lenna", image);
cv::imshow("Warped lenna", warpedImage);
cv::waitKey();
cv::destroyAllWindows();
return 0;
}
Thanks to everyone for this post, it has been super useful. However, I have found some black lines left and up (using Rose's python version) when rotating 90º. The problem seemed to be some int() roundings. In addition to that, I have changed the sign of the angle to make it grow clockwise.
def rotate_image(image, angle):
'''Rotate image "angle" degrees.
How it works:
- Creates a blank image that fits any rotation of the image. To achieve
this, set the height and width to be the image's diagonal.
- Copy the original image to the center of this blank image
- Rotate using warpAffine, using the newly created image's center
(the enlarged blank image center)
- Translate the four corners of the source image in the enlarged image
using homogenous multiplication of the rotation matrix.
- Crop the image according to these transformed corners
'''
diagonal = int(math.ceil(math.sqrt(pow(image.shape[0], 2) + pow(image.shape[1], 2))))
offset_x = (diagonal - image.shape[0])/2
offset_y = (diagonal - image.shape[1])/2
dst_image = np.zeros((diagonal, diagonal, 3), dtype='uint8')
image_center = (float(diagonal-1)/2, float(diagonal-1)/2)
R = cv2.getRotationMatrix2D(image_center, -angle, 1.0)
dst_image[offset_x:(offset_x + image.shape[0]), offset_y:(offset_y + image.shape[1]), :] = image
dst_image = cv2.warpAffine(dst_image, R, (diagonal, diagonal), flags=cv2.INTER_LINEAR)
# Calculate the rotated bounding rect
x0 = offset_x
x1 = offset_x + image.shape[0]
x2 = offset_x + image.shape[0]
x3 = offset_x
y0 = offset_y
y1 = offset_y
y2 = offset_y + image.shape[1]
y3 = offset_y + image.shape[1]
corners = np.zeros((3,4))
corners[0,0] = x0
corners[0,1] = x1
corners[0,2] = x2
corners[0,3] = x3
corners[1,0] = y0
corners[1,1] = y1
corners[1,2] = y2
corners[1,3] = y3
corners[2:] = 1
c = np.dot(R, corners)
x = int(round(c[0,0]))
y = int(round(c[1,0]))
left = x
right = x
up = y
down = y
for i in range(4):
x = c[0,i]
y = c[1,i]
if (x < left): left = x
if (x > right): right = x
if (y < up): up = y
if (y > down): down = y
h = int(round(down - up))
w = int(round(right - left))
left = int(round(left))
up = int(round(up))
cropped = np.zeros((w, h, 3), dtype='uint8')
cropped[:, :, :] = dst_image[left:(left+w), up:(up+h), :]
return cropped
Go version (using gocv) of #robula and #remi-cuingnet
func rotateImage(mat *gocv.Mat, angle float64) *gocv.Mat {
height := mat.Rows()
width := mat.Cols()
imgCenter := image.Point{X: width/2, Y: height/2}
rotationMat := gocv.GetRotationMatrix2D(imgCenter, -angle, 1.0)
absCos := math.Abs(rotationMat.GetDoubleAt(0, 0))
absSin := math.Abs(rotationMat.GetDoubleAt(0, 1))
boundW := float64(height) * absSin + float64(width) * absCos
boundH := float64(height) * absCos + float64(width) * absSin
rotationMat.SetDoubleAt(0, 2, rotationMat.GetDoubleAt(0, 2) + (boundW / 2) - float64(imgCenter.X))
rotationMat.SetDoubleAt(1, 2, rotationMat.GetDoubleAt(1, 2) + (boundH / 2) - float64(imgCenter.Y))
gocv.WarpAffine(*mat, mat, rotationMat, image.Point{ X: int(boundW), Y: int(boundH) })
return mat
}
I rotate in the same matrice in-memory, make a new matrice if you don't want to alter it
For anyone using Emgu.CV or OpenCvSharp wrapper in .NET, there's a C# implement of Lars Schillingmann's answer:
Emgu.CV:
using Emgu.CV;
using Emgu.CV.CvEnum;
using Emgu.CV.Structure;
public static class MatExtension
{
/// <summary>
/// <see>https://stackoverflow.com/questions/22041699/rotate-an-image-without-cropping-in-opencv-in-c/75451191#75451191</see>
/// </summary>
public static Mat Rotate(this Mat src, float degrees)
{
degrees = -degrees; // counter-clockwise to clockwise
var center = new PointF((src.Width - 1) / 2f, (src.Height - 1) / 2f);
var rotationMat = new Mat();
CvInvoke.GetRotationMatrix2D(center, degrees, 1, rotationMat);
var boundingRect = new RotatedRect(new(), src.Size, degrees).MinAreaRect();
rotationMat.Set(0, 2, rotationMat.Get<double>(0, 2) + (boundingRect.Width / 2f) - (src.Width / 2f));
rotationMat.Set(1, 2, rotationMat.Get<double>(1, 2) + (boundingRect.Height / 2f) - (src.Height / 2f));
var rotatedSrc = new Mat();
CvInvoke.WarpAffine(src, rotatedSrc, rotationMat, boundingRect.Size);
return rotatedSrc;
}
/// <summary>
/// <see>https://stackoverflow.com/questions/32255440/how-can-i-get-and-set-pixel-values-of-an-emgucv-mat-image/69537504#69537504</see>
/// </summary>
public static unsafe void Set<T>(this Mat mat, int row, int col, T value) where T : struct =>
_ = new Span<T>(mat.DataPointer.ToPointer(), mat.Rows * mat.Cols * mat.ElementSize)
{
[(row * mat.Cols) + col] = value
};
public static unsafe T Get<T>(this Mat mat, int row, int col) where T : struct =>
new ReadOnlySpan<T>(mat.DataPointer.ToPointer(), mat.Rows * mat.Cols * mat.ElementSize)
[(row * mat.Cols) + col];
}
OpenCvSharp:
OpenCvSharp already has Mat.Set<> method that functions same as mat.at<> in the original OpenCV, so we don't have to copy these methods from How can I get and set pixel values of an EmguCV Mat image?
using OpenCvSharp;
public static class MatExtension
{
/// <summary>
/// <see>https://stackoverflow.com/questions/22041699/rotate-an-image-without-cropping-in-opencv-in-c/75451191#75451191</see>
/// </summary>
public static Mat Rotate(this Mat src, float degrees)
{
degrees = -degrees; // counter-clockwise to clockwise
var center = new Point2f((src.Width - 1) / 2f, (src.Height - 1) / 2f);
var rotationMat = Cv2.GetRotationMatrix2D(center, degrees, 1);
var boundingRect = new RotatedRect(new(), new Size2f(src.Width, src.Height), degrees).BoundingRect();
rotationMat.Set(0, 2, rotationMat.Get<double>(0, 2) + (boundingRect.Width / 2f) - (src.Width / 2f));
rotationMat.Set(1, 2, rotationMat.Get<double>(1, 2) + (boundingRect.Height / 2f) - (src.Height / 2f));
var rotatedSrc = new Mat();
Cv2.WarpAffine(src, rotatedSrc, rotationMat, boundingRect.Size);
return rotatedSrc;
}
}
Also, you may want to mutate the src param instead of returning a new clone of it during rotation, for that you can just set the det param of WrapAffine() as the same with src: c++, opencv: Is it safe to use the same Mat for both source and destination images in filtering operation?
CvInvoke.WarpAffine(src, src, rotationMat, boundingRect.Size);
This is being called as in-place mode: https://answers.opencv.org/question/24/do-all-opencv-functions-support-in-place-mode-for-their-arguments/
Can the OpenCV function cvtColor be used to convert a matrix in place?
If it is just to rotate 90 degrees, maybe this code could be useful.
Mat img = imread("images.jpg");
Mat rt(img.rows, img.rows, CV_8U);
Point2f pc(img.cols / 2.0, img.rows / 2.0);
Mat r = getRotationMatrix2D(pc, 90, 1);
warpAffine(img, rt, r, rt.size());
imshow("rotated", rt);
Hope it's useful.
By the way, for 90º rotations only, here is a more efficient + accurate function:
def rotate_image_90(image, angle):
angle = -angle
rotated_image = image
if angle == 0:
pass
elif angle == 90:
rotated_image = np.rot90(rotated_image)
elif angle == 180 or angle == -180:
rotated_image = np.rot90(rotated_image)
rotated_image = np.rot90(rotated_image)
elif angle == -90:
rotated_image = np.rot90(rotated_image)
rotated_image = np.rot90(rotated_image)
rotated_image = np.rot90(rotated_image)
return rotated_image

Clip line to screen coordinates

I have line that is defined as two points.
start = (xs,ys)
end = (xe, ye)
Drawing function that I'm using Only accepts lines that are fully in screen coordinates.
Screen size is (xSize, ySize).
Top left corner is (0,0). Bottom right corner is (xSize, ySize).
Some other funcions gives me line that that is defined for example as start(-50, -15) end(5000, 200). So it's ends are outside of screen size.
In C++
struct Vec2
{
int x, y
};
Vec2 start, end //This is all little bit pseudo code
Vec2 screenSize;//You can access coordinates like start.x end.y
How can I calculate new start and endt that is at the screen edge, not outside screen.
I know how to do it on paper. But I can't transfer it to c++.
On paper I'm sershing for point that belongs to edge and line. But it is to much calculations for c++.
Can you help?
There are many line clipping algorithms like:
Cohen–Sutherland wikipedia page with implementation
Liang–Barsky wikipedia page
Nicholl–Lee–Nicholl (NLN)
and many more. see Line Clipping on wikipedia
[EDIT1]
See below figure:
there are 3 kinds of start point:
sx > 0 and sy < 0 (red line)
sx < 0 and sy > 0 (yellow line)
sx < 0 and sy < 0 (green and violet lines)
In situations 1 and 2 simply find Xintersect and Yintersect respectively and choose them as new start point.
As you can see, there are 2 kinds of lines in situation 3. In this situation find Xintersect and Yintersect and choose the intersect point near the end point which is the point that has minimum distance to endPoint.
min(distance(Xintersect, endPoint), distance(Yintersect, endPoint))
[EDIT2]
// Liang-Barsky function by Daniel White # http://www.skytopia.com/project/articles/compsci/clipping.html
// This function inputs 8 numbers, and outputs 4 new numbers (plus a boolean value to say whether the clipped line is drawn at all).
//
bool LiangBarsky (double edgeLeft, double edgeRight, double edgeBottom, double edgeTop, // Define the x/y clipping values for the border.
double x0src, double y0src, double x1src, double y1src, // Define the start and end points of the line.
double &x0clip, double &y0clip, double &x1clip, double &y1clip) // The output values, so declare these outside.
{
double t0 = 0.0; double t1 = 1.0;
double xdelta = x1src-x0src;
double ydelta = y1src-y0src;
double p,q,r;
for(int edge=0; edge<4; edge++) { // Traverse through left, right, bottom, top edges.
if (edge==0) { p = -xdelta; q = -(edgeLeft-x0src); }
if (edge==1) { p = xdelta; q = (edgeRight-x0src); }
if (edge==2) { p = -ydelta; q = -(edgeBottom-y0src);}
if (edge==3) { p = ydelta; q = (edgeTop-y0src); }
r = q/p;
if(p==0 && q<0) return false; // Don't draw line at all. (parallel line outside)
if(p<0) {
if(r>t1) return false; // Don't draw line at all.
else if(r>t0) t0=r; // Line is clipped!
} else if(p>0) {
if(r<t0) return false; // Don't draw line at all.
else if(r<t1) t1=r; // Line is clipped!
}
}
x0clip = x0src + t0*xdelta;
y0clip = y0src + t0*ydelta;
x1clip = x0src + t1*xdelta;
y1clip = y0src + t1*ydelta;
return true; // (clipped) line is drawn
}
Here is a function I wrote. It cycles through all 4 planes (left, top, right, bottom) and clips each point by the plane.
// Clips a line segment to an axis-aligned rectangle
// Returns true if clipping is successful
// Returns false if line segment lies outside the rectangle
bool clipLineToRect(int a[2], int b[2],
int xmin, int ymin, int xmax, int ymax)
{
int mins[2] = {xmin, ymin};
int maxs[2] = {xmax, ymax};
int normals[2] = {1, -1};
for (int axis=0; axis<2; axis++) {
for (int plane=0; plane<2; plane++) {
// Check both points
for (int pt=1; pt<=2; pt++) {
int* pt1 = pt==1 ? a : b;
int* pt2 = pt==1 ? b : a;
// If both points are outside the same plane, the line is
// outside the rectangle
if ( (a[0]<xmin && b[0]<xmin) || (a[0]>xmax && b[0]>xmax) ||
(a[1]<ymin && b[1]<ymin) || (a[1]>ymax && b[1]>ymax)) {
return false;
}
const int n = normals[plane];
if ( (n==1 && pt1[axis]<mins[axis]) || // check left/top plane
(n==-1 && pt1[axis]>maxs[axis]) ) { // check right/bottom plane
// Calculate interpolation factor t using ratio of signed distance
// of each point from the plane
const float p = (n==1) ? mins[axis] : maxs[axis];
const float q1 = pt1[axis];
const float q2 = pt2[axis];
const float d1 = n * (q1-p);
const float d2 = n * (q2-p);
const float t = d1 / (d1-d2);
// t should always be between 0 and 1
if (t<0 || t >1) {
return false;
}
// Interpolate to find the new point
pt1[0] = (int)(pt1[0] + (pt2[0] - pt1[0]) * t );
pt1[1] = (int)(pt1[1] + (pt2[1] - pt1[1]) * t );
}
}
}
}
return true;
}
Example Usage:
void testClipLineToRect()
{
int screenWidth = 320;
int screenHeight = 240;
int xmin=0;
int ymin=0;
int xmax=screenWidth-1;
int ymax=screenHeight-1;
int a[2] = {-10, 10};
int b[2] = {300, 250};
printf("Before clipping:\n\ta={%d, %d}\n\tb=[%d, %d]\n",
a[0], a[1], b[0], b[1]);
if (clipLineToRect(a, b, xmin, ymin, xmax, ymax)) {
printf("After clipping:\n\ta={%d, %d}\n\tb=[%d, %d]\n",
a[0], a[1], b[0], b[1]);
}
else {
printf("clipLineToRect returned false\n");
}
}
Output:
Before clipping:
a={-10, 10}
b=[300, 250]
After clipping:
a={0, 17}
b=[285, 239]

Is there an easy way/algorithm to match 2 clouds of 2D points?

I am wondering if there is an easy way to match (register) 2 clouds of 2d points.
Let's say I have an object represented by points and an cluttered 2nd image with the object points and noise (noise in a way of points that are useless).
Basically the object can be 2d rotated as well as translated and scaled.
I know there is the ICP - Algorithm but I think that this is not a good approach due to high noise.
I hope that you understand what i mean. please ask if (im sure it is) anything is unclear.
cheers
Here is the function that finds translation and rotation. Generalization to scaling, weighted points, and RANSAC are straight forward. I used openCV library for visualization and SVD. The function below combines data generation, Unit Test , and actual solution.
// rotation and translation in 2D from point correspondences
void rigidTransform2D(const int N) {
// Algorithm: http://igl.ethz.ch/projects/ARAP/svd_rot.pdf
const bool debug = false; // print more debug info
const bool add_noise = true; // add noise to imput and output
srand(time(NULL)); // randomize each time
/*********************************
* Creat data with some noise
**********************************/
// Simulated transformation
Point2f T(1.0f, -2.0f);
float a = 30.0; // [-180, 180], see atan2(y, x)
float noise_level = 0.1f;
cout<<"True parameters: rot = "<<a<<"deg., T = "<<T<<
"; noise level = "<<noise_level<<endl;
// noise
vector<Point2f> noise_src(N), noise_dst(N);
for (int i=0; i<N; i++) {
noise_src[i] = Point2f(randf(noise_level), randf(noise_level));
noise_dst[i] = Point2f(randf(noise_level), randf(noise_level));
}
// create data with noise
vector<Point2f> src(N), dst(N);
float Rdata = 10.0f; // radius of data
float cosa = cos(a*DEG2RAD);
float sina = sin(a*DEG2RAD);
for (int i=0; i<N; i++) {
// src
float x1 = randf(Rdata);
float y1 = randf(Rdata);
src[i] = Point2f(x1,y1);
if (add_noise)
src[i] += noise_src[i];
// dst
float x2 = x1*cosa - y1*sina;
float y2 = x1*sina + y1*cosa;
dst[i] = Point2f(x2,y2) + T;
if (add_noise)
dst[i] += noise_dst[i];
if (debug)
cout<<i<<": "<<src[i]<<"---"<<dst[i]<<endl;
}
// Calculate data centroids
Scalar centroid_src = mean(src);
Scalar centroid_dst = mean(dst);
Point2f center_src(centroid_src[0], centroid_src[1]);
Point2f center_dst(centroid_dst[0], centroid_dst[1]);
if (debug)
cout<<"Centers: "<<center_src<<", "<<center_dst<<endl;
/*********************************
* Visualize data
**********************************/
// Visualization
namedWindow("data", 1);
float w = 400, h = 400;
Mat Mdata(w, h, CV_8UC3); Mdata = Scalar(0);
Point2f center_img(w/2, h/2);
float scl = 0.4*min(w/Rdata, h/Rdata); // compensate for noise
scl/=sqrt(2); // compensate for rotation effect
Point2f dT = (center_src+center_dst)*0.5; // compensate for translation
for (int i=0; i<N; i++) {
Point2f p1(scl*(src[i] - dT));
Point2f p2(scl*(dst[i] - dT));
// invert Y axis
p1.y = -p1.y; p2.y = -p2.y;
// add image center
p1+=center_img; p2+=center_img;
circle(Mdata, p1, 1, Scalar(0, 255, 0));
circle(Mdata, p2, 1, Scalar(0, 0, 255));
line(Mdata, p1, p2, Scalar(100, 100, 100));
}
/*********************************
* Get 2D rotation and translation
**********************************/
markTime();
// subtract centroids from data
for (int i=0; i<N; i++) {
src[i] -= center_src;
dst[i] -= center_dst;
}
// compute a covariance matrix
float Cxx = 0.0, Cxy = 0.0, Cyx = 0.0, Cyy = 0.0;
for (int i=0; i<N; i++) {
Cxx += src[i].x*dst[i].x;
Cxy += src[i].x*dst[i].y;
Cyx += src[i].y*dst[i].x;
Cyy += src[i].y*dst[i].y;
}
Mat Mcov = (Mat_<float>(2, 2)<<Cxx, Cxy, Cyx, Cyy);
if (debug)
cout<<"Covariance Matrix "<<Mcov<<endl;
// SVD
cv::SVD svd;
svd = SVD(Mcov, SVD::FULL_UV);
if (debug) {
cout<<"U = "<<svd.u<<endl;
cout<<"W = "<<svd.w<<endl;
cout<<"V transposed = "<<svd.vt<<endl;
}
// rotation = V*Ut
Mat V = svd.vt.t();
Mat Ut = svd.u.t();
float det_VUt = determinant(V*Ut);
Mat W = (Mat_<float>(2, 2)<<1.0, 0.0, 0.0, det_VUt);
float rot[4];
Mat R_est(2, 2, CV_32F, rot);
R_est = V*W*Ut;
if (debug)
cout<<"Rotation matrix: "<<R_est<<endl;
float cos_est = rot[0];
float sin_est = rot[2];
float ang = atan2(sin_est, cos_est);
// translation = mean_dst - R*mean_src
Point2f center_srcRot = Point2f(
cos_est*center_src.x - sin_est*center_src.y,
sin_est*center_src.x + cos_est*center_src.y);
Point2f T_est = center_dst - center_srcRot;
// RMSE
double RMSE = 0.0;
for (int i=0; i<N; i++) {
Point2f dst_est(
cos_est*src[i].x - sin_est*src[i].y,
sin_est*src[i].x + cos_est*src[i].y);
RMSE += SQR(dst[i].x - dst_est.x) + SQR(dst[i].y - dst_est.y);
}
if (N>0)
RMSE = sqrt(RMSE/N);
// Final estimate msg
cout<<"Estimate = "<<ang*RAD2DEG<<"deg., T = "<<T_est<<"; RMSE = "<<RMSE<<endl;
// show image
printTime(1);
imshow("data", Mdata);
waitKey(-1);
return;
} // rigidTransform2D()
// --------------------------- 3DOF
// calculates squared error from two point mapping; assumes rotation around Origin.
inline float sqErr_3Dof(Point2f p1, Point2f p2,
float cos_alpha, float sin_alpha, Point2f T) {
float x2_est = T.x + cos_alpha * p1.x - sin_alpha * p1.y;
float y2_est = T.y + sin_alpha * p1.x + cos_alpha * p1.y;
Point2f p2_est(x2_est, y2_est);
Point2f dp = p2_est-p2;
float sq_er = dp.dot(dp); // squared distance
//cout<<dp<<endl;
return sq_er;
}
// calculate RMSE for point-to-point metrics
float RMSE_3Dof(const vector<Point2f>& src, const vector<Point2f>& dst,
const float* param, const bool* inliers, const Point2f center) {
const bool all_inliers = (inliers==NULL); // handy when we run QUADRTATIC will all inliers
unsigned int n = src.size();
assert(n>0 && n==dst.size());
float ang_rad = param[0];
Point2f T(param[1], param[2]);
float cos_alpha = cos(ang_rad);
float sin_alpha = sin(ang_rad);
double RMSE = 0.0;
int ninliers = 0;
for (unsigned int i=0; i<n; i++) {
if (all_inliers || inliers[i]) {
RMSE += sqErr_3Dof(src[i]-center, dst[i]-center, cos_alpha, sin_alpha, T);
ninliers++;
}
}
//cout<<"RMSE = "<<RMSE<<endl;
if (ninliers>0)
return sqrt(RMSE/ninliers);
else
return LARGE_NUMBER;
}
// Sets inliers and returns their count
inline int setInliers3Dof(const vector<Point2f>& src, const vector <Point2f>& dst,
bool* inliers,
const float* param,
const float max_er,
const Point2f center) {
float ang_rad = param[0];
Point2f T(param[1], param[2]);
// set inliers
unsigned int ninliers = 0;
unsigned int n = src.size();
assert(n>0 && n==dst.size());
float cos_ang = cos(ang_rad);
float sin_ang = sin(ang_rad);
float max_sqErr = max_er*max_er; // comparing squared values
if (inliers==NULL) {
// just get the number of inliers (e.g. after QUADRATIC fit only)
for (unsigned int i=0; i<n; i++) {
float sqErr = sqErr_3Dof(src[i]-center, dst[i]-center, cos_ang, sin_ang, T);
if ( sqErr < max_sqErr)
ninliers++;
}
} else {
// get the number of inliers and set them (e.g. for RANSAC)
for (unsigned int i=0; i<n; i++) {
float sqErr = sqErr_3Dof(src[i]-center, dst[i]-center, cos_ang, sin_ang, T);
if ( sqErr < max_sqErr) {
inliers[i] = 1;
ninliers++;
} else {
inliers[i] = 0;
}
}
}
return ninliers;
}
// fits 3DOF (rotation and translation in 2D) with least squares.
float fit3DofQUADRATICold(const vector<Point2f>& src, const vector<Point2f>& dst,
float* param, const bool* inliers, const Point2f center) {
const bool all_inliers = (inliers==NULL); // handy when we run QUADRTATIC will all inliers
unsigned int n = src.size();
assert(dst.size() == n);
// count inliers
int ninliers;
if (all_inliers) {
ninliers = n;
} else {
ninliers = 0;
for (unsigned int i=0; i<n; i++){
if (inliers[i])
ninliers++;
}
}
// under-dermined system
if (ninliers<2) {
// param[0] = 0.0f; // ?
// param[1] = 0.0f;
// param[2] = 0.0f;
return LARGE_NUMBER;
}
/*
* x1*cosx(a)-y1*sin(a) + Tx = X1
* x1*sin(a)+y1*cos(a) + Ty = Y1
*
* approximation for small angle a (radians) sin(a)=a, cos(a)=1;
*
* x1*1 - y1*a + Tx = X1
* x1*a + y1*1 + Ty = Y1
*
* in matrix form M1*h=M2
*
* 2n x 4 4 x 1 2n x 1
*
* -y1 1 0 x1 * a = X1
* x1 0 1 y1 Tx Y1
* Ty
* 1=Z
* ----------------------------
* src1 res src2
*/
// 4 x 1
float res_ar[4]; // alpha, Tx, Ty, 1
Mat res(4, 1, CV_32F, res_ar); // 4 x 1
// 2n x 4
Mat src1(2*ninliers, 4, CV_32F); // 2n x 4
// 2n x 1
Mat src2(2*ninliers, 1, CV_32F); // 2n x 1: [X1, Y1, X2, Y2, X3, Y3]'
for (unsigned int i=0, row_cnt = 0; i<n; i++) {
// use inliers only
if (all_inliers || inliers[i]) {
float x = src[i].x - center.x;
float y = src[i].y - center.y;
// first row
// src1
float* rowPtr = src1.ptr<float>(row_cnt);
rowPtr[0] = -y;
rowPtr[1] = 1.0f;
rowPtr[2] = 0.0f;
rowPtr[3] = x;
// src2
src2.at<float> (0, row_cnt) = dst[i].x - center.x;
// second row
row_cnt++;
// src1
rowPtr = src1.ptr<float>(row_cnt);
rowPtr[0] = x;
rowPtr[1] = 0.0f;
rowPtr[2] = 1.0f;
rowPtr[3] = y;
// src2
src2.at<float> (0, row_cnt) = dst[i].y - center.y;
}
}
cv::solve(src1, src2, res, DECOMP_SVD);
// estimators
float alpha_est;
Point2f T_est;
// original
alpha_est = res.at<float>(0, 0);
T_est = Point2f(res.at<float>(1, 0), res.at<float>(2, 0));
float Z = res.at<float>(3, 0);
if (abs(Z-1.0) > 0.1) {
//cout<<"Bad Z in fit3DOF(), Z should be close to 1.0 = "<<Z<<endl;
//return LARGE_NUMBER;
}
param[0] = alpha_est; // rad
param[1] = T_est.x;
param[2] = T_est.y;
// calculate RMSE
float RMSE = RMSE_3Dof(src, dst, param, inliers, center);
return RMSE;
} // fit3DofQUADRATICOLd()
// fits 3DOF (rotation and translation in 2D) with least squares.
float fit3DofQUADRATIC(const vector<Point2f>& src_, const vector<Point2f>& dst_,
float* param, const bool* inliers, const Point2f center) {
const bool debug = false; // print more debug info
const bool all_inliers = (inliers==NULL); // handy when we run QUADRTATIC will all inliers
assert(dst_.size() == src_.size());
int N = src_.size();
// collect inliers
vector<Point2f> src, dst;
int ninliers;
if (all_inliers) {
ninliers = N;
src = src_; // copy constructor
dst = dst_;
} else {
ninliers = 0;
for (int i=0; i<N; i++){
if (inliers[i]) {
ninliers++;
src.push_back(src_[i]);
dst.push_back(dst_[i]);
}
}
}
if (ninliers<2) {
param[0] = 0.0f; // default return when there is not enough points
param[1] = 0.0f;
param[2] = 0.0f;
return LARGE_NUMBER;
}
/* Algorithm: Least-Square Rigid Motion Using SVD by Olga Sorkine
* http://igl.ethz.ch/projects/ARAP/svd_rot.pdf
*
* Subtract centroids, calculate SVD(cov),
* R = V[1, det(VU')]'U', T = mean_q-R*mean_p
*/
// Calculate data centroids
Scalar centroid_src = mean(src);
Scalar centroid_dst = mean(dst);
Point2f center_src(centroid_src[0], centroid_src[1]);
Point2f center_dst(centroid_dst[0], centroid_dst[1]);
if (debug)
cout<<"Centers: "<<center_src<<", "<<center_dst<<endl;
// subtract centroids from data
for (int i=0; i<ninliers; i++) {
src[i] -= center_src;
dst[i] -= center_dst;
}
// compute a covariance matrix
float Cxx = 0.0, Cxy = 0.0, Cyx = 0.0, Cyy = 0.0;
for (int i=0; i<ninliers; i++) {
Cxx += src[i].x*dst[i].x;
Cxy += src[i].x*dst[i].y;
Cyx += src[i].y*dst[i].x;
Cyy += src[i].y*dst[i].y;
}
Mat Mcov = (Mat_<float>(2, 2)<<Cxx, Cxy, Cyx, Cyy);
Mcov /= (ninliers-1);
if (debug)
cout<<"Covariance-like Matrix "<<Mcov<<endl;
// SVD of covariance
cv::SVD svd;
svd = SVD(Mcov, SVD::FULL_UV);
if (debug) {
cout<<"U = "<<svd.u<<endl;
cout<<"W = "<<svd.w<<endl;
cout<<"V transposed = "<<svd.vt<<endl;
}
// rotation (V*Ut)
Mat V = svd.vt.t();
Mat Ut = svd.u.t();
float det_VUt = determinant(V*Ut);
Mat W = (Mat_<float>(2, 2)<<1.0, 0.0, 0.0, det_VUt);
float rot[4];
Mat R_est(2, 2, CV_32F, rot);
R_est = V*W*Ut;
if (debug)
cout<<"Rotation matrix: "<<R_est<<endl;
float cos_est = rot[0];
float sin_est = rot[2];
float ang = atan2(sin_est, cos_est);
// translation (mean_dst - R*mean_src)
Point2f center_srcRot = Point2f(
cos_est*center_src.x - sin_est*center_src.y,
sin_est*center_src.x + cos_est*center_src.y);
Point2f T_est = center_dst - center_srcRot;
// Final estimate msg
if (debug)
cout<<"Estimate = "<<ang*RAD2DEG<<"deg., T = "<<T_est<<endl;
param[0] = ang; // rad
param[1] = T_est.x;
param[2] = T_est.y;
// calculate RMSE
float RMSE = RMSE_3Dof(src_, dst_, param, inliers, center);
return RMSE;
} // fit3DofQUADRATIC()
// RANSAC fit in 3DOF: 1D rot and 2D translation (maximizes the number of inliers)
// NOTE: no data normalization is currently performed
float fit3DofRANSAC(const vector<Point2f>& src, const vector<Point2f>& dst,
float* best_param, bool* inliers,
const Point2f center ,
const float inlierMaxEr,
const int niter) {
const int ITERATION_TO_SETTLE = 2; // iterations to settle inliers and param
const float INLIERS_RATIO_OK = 0.95f; // stopping criterion
// size of data vector
unsigned int N = src.size();
assert(N==dst.size());
// unrealistic case
if(N<2) {
best_param[0] = 0.0f; // ?
best_param[1] = 0.0f;
best_param[2] = 0.0f;
return LARGE_NUMBER;
}
unsigned int ninliers; // current number of inliers
unsigned int best_ninliers = 0; // number of inliers
float best_rmse = LARGE_NUMBER; // error
float cur_rmse; // current distance error
float param[3]; // rad, Tx, Ty
vector <Point2f> src_2pt(2), dst_2pt(2);// min set of 2 points (1 correspondence generates 2 equations)
srand (time(NULL));
// iterations
for (int iter = 0; iter<niter; iter++) {
#ifdef DEBUG_RANSAC
cout<<"iteration "<<iter<<": ";
#endif
// 1. Select a random set of 2 points (not obligatory inliers but valid)
int i1, i2;
i1 = rand() % N; // [0, N[
i2 = i1;
while (i2==i1) {
i2 = rand() % N;
}
src_2pt[0] = src[i1]; // corresponding points
src_2pt[1] = src[i2];
dst_2pt[0] = dst[i1];
dst_2pt[1] = dst[i2];
bool two_inliers[] = {true, true};
// 2. Quadratic fit for 2 points
cur_rmse = fit3DofQUADRATIC(src_2pt, dst_2pt, param, two_inliers, center);
// 3. Recalculate to settle params and inliers using a larger set
for (int iter2=0; iter2<ITERATION_TO_SETTLE; iter2++) {
ninliers = setInliers3Dof(src, dst, inliers, param, inlierMaxEr, center); // changes inliers
cur_rmse = fit3DofQUADRATIC(src, dst, param, inliers, center); // changes cur_param
}
// potential ill-condition or large error
if (ninliers<2) {
#ifdef DEBUG_RANSAC
cout<<" !!! less than 2 inliers "<<endl;
#endif
continue;
} else {
#ifdef DEBUG_RANSAC
cout<<" "<<ninliers<<" inliers; ";
#endif
}
#ifdef DEBUG_RANSAC
cout<<"; recalculate: RMSE = "<<cur_rmse<<", "<<ninliers <<" inliers";
#endif
// 4. found a better solution?
if (ninliers > best_ninliers) {
best_ninliers = ninliers;
best_param[0] = param[0];
best_param[1] = param[1];
best_param[2] = param[2];
best_rmse = cur_rmse;
#ifdef DEBUG_RANSAC
cout<<" --- Solution improved: "<<
best_param[0]<<", "<<best_param[1]<<", "<<param[2]<<endl;
#endif
// exit condition
float inlier_ratio = (float)best_ninliers/N;
if (inlier_ratio > INLIERS_RATIO_OK) {
#ifdef DEBUG_RANSAC
cout<<"Breaking early after "<< iter+1<<
" iterations; inlier ratio = "<<inlier_ratio<<endl;
#endif
break;
}
} else {
#ifdef DEBUG_RANSAC
cout<<endl;
#endif
}
} // iterations
// 5. recreate inliers for the best parameters
ninliers = setInliers3Dof(src, dst, inliers, best_param, inlierMaxEr, center);
return best_rmse;
} // fit3DofRANSAC()
Let me first make sure I'm interpreting your question correctly. You have two sets of 2D points, one of which contains all "good" points corresponding to some object of interest, and one of which contains those points under an affine transformation with noisy points added. Right?
If that's correct, then there is a fairly reliable and efficient way to both reject noisy points and determine the transformation between your points of interest. The algorithm that is usually used to reject noisy points ("outliers") is known as RANSAC, and the algorithm used to determine the transformation can take several forms, but the most current state of the art is known as the five-point algorithm and can be found here -- a MATLAB implementation can be found here.
Unfortunately I don't know of a mature implementation of both of those combined; you'll probably have to do some work of your own to implement RANSAC and integrate it with the five point algorithm.
Edit:
Actually, OpenCV has an implementation that is overkill for your task (meaning it will work but will take more time than necessary) but is ready to work out of the box. The function of interest is called cv::findFundamentalMat.
I believe you are looking for something like David Lowe's SIFT (Scale Invariant Feature Transform). Other option is SURF (SIFT is patent protected). The OpenCV computer library presents a SURF implementation
I would try and use distance geometry (http://en.wikipedia.org/wiki/Distance_geometry) for this
Generate a scalar for each point by summing its distances to all neighbors within a certain radius. Though not perfect, this will be good discriminator for each point.
Then put all the scalars in a map that allows a point (p) to be retrieve by its scalar (s) plus/minus some delta
M(s+delta) = p (e.g K-D Tree) (http://en.wikipedia.org/wiki/Kd-tree)
Put all the reference set of 2D points in the map
On the other (test) set of 2D points:
foreach test scaling (esp if you have a good idea what typical scaling values are)
...scale each point by S
...recompute the scalars of the test set of points
......for each point P in test set (or perhaps a sample for faster method)
.........lookup point in reference scalar map within some delta
.........discard P if no mapping found
.........else foreach P' point found
............examine neighbors of P and see if they have corresponding scalars in the reference map within some delta (i.e reference point has neighbors with approx same value)
......... if all points tested have a mapping in the reference set, you have found a mapping of test point P onto reference point P' -> record mapping of test point to reference point
......discard scaling if no mappings recorded
Note this is trivially parallelized in several different places
This is off the top of my head, drawing from research I did years ago. It lacks fine details but the general idea is clear: find points in the noisy (test) graph whose distances to their closest neighbors are roughly the same as the reference set. Noisy graphs will have to measure the distances with a larger allowed error that less noisy graphs.
The algorithm works perfectly for graphs with no noise.
Edit: there is a refinement for the algorithm that doesn't require looking at different scalings. When computing the scalar for each point, use a relative distance measure instead. This will be invariant of transform
From C++, you could use ITK to do the image registration. It includes many registration functions that will work in the presence of noise.
The KLT (Kanade Lucas Tomasi) Feature Tracker makes a Affine Consistency Check of tracked features. The Affine Consistency Check takes into account translation, rotation and scaling. I don't know if it is of help to you, because you can't use the function (which calculates the affine transformation of a rectangular region) directly. But maybe you can learn from the documentation and source-code, how the affine transformation can be calculated and adapt it to your problem (clouds of points instead of a rectangular region).
You want want the Denton-Beveridge point matching algorithm. Source code at the bottom of the page linked below, and there is also a paper that explain the algorithm and why Ransac is a bad choice for this problem.
http://jasondenton.me/pntmatch.html