Recently, I created a ROS node which would listen to an image topic and start stitching them to create a mosaic. This was done using multiple methods. Two methods were OpenCV implemented (this and this) whereas a third method was manually implemented based on this paper. The basic principle of the node was simple. It takes a global image, and a local image, and runs SURF feature detection on them. It then matches the two images using FLANN based Matcher. It then takes the corresponding points, and runs RANSAC on them to find best corresponding points. It then uses these corresponding points to calculate a Transform which would map the second image on the global image. This is shown below. The code has been modified to make understandable.
double bestconerror = 99999999.0; int consize = 0; vector model(3);
model[0] = 0.0; model[1] = 0.0; model[2] = 0.0;
double omega=0.0; double Tx=0.0; double Ty=0.0;
for (int i = 0; i {
//Chooses three corresponding points and calculates
//omega, Tx, and Ty
vector myvector;
myvector = getrandom(localB.size()-1); //Returns a vector of 3 different random points
int p1 = myvector[0]; int p2 = myvector[1]; int p3 = myvector[2];
Point2f A1= globalA[p1]; Point2f A2= globalA[p2]; Point2f A3= globalA[p3];
Point2f B1= localB[p1]; Point2f B2= localB[p2]; Point2f B3= localB[p3];
double Axmean = (A1.x + A2.x + A3.x)/3.0; double Bxmean = (B1.x + B2.x + B3.x)/3.0;
double Aymean = (A1.y + A2.y + A3.y)/3.0; double Bymean = (B1.y + B2.y + B3.y)/3.0;
double Saxbx = (A1.x-Axmean)*(B1.x-Bxmean) + (A2.x-Axmean)*(B2.x-Bxmean) + (A3.x-Axmean)*(B3.x-Bxmean);
double Saxby = (A1.x-Axmean)*(B1.y-Bymean) + (A2.x-Axmean)*(B2.y-Bymean) + (A3.x-Axmean)*(B3.y-Bymean);
double Saybx = (A1.y-Aymean)*(B1.x-Bxmean) + (A2.y-Aymean)*(B2.x-Bxmean) + (A3.y-Aymean)*(B3.x-Bxmean);
double Sayby = (A1.y-Aymean)*(B1.y-Bymean) + (A2.y-Aymean)*(B2.y-Bymean) + (A3.y-Aymean)*(B3.y-Bymean);
omega = atan2((Saxby-Saybx),(Saxbx+Sayby));
Tx = Bxmean - ((Axmean*cos(omega)) - (Aymean*sin(omega)));
Ty = Bymean - ((Axmean*sin(omega)) + (Aymean*cos(omega)));
int localcon = 0;
//Finds the concensus set
for (int i = 0; i {
double ls = (globalA[i].x * cos(omega))-(globalA[i].y * sin(omega)) + Tx - localB[i].x;
double rs = (globalA[i].x * sin(omega))+(globalA[i].y * cos(omega)) + Ty - localB[i].y;
double error = sqrt((ls*ls)+(rs*rs));
if (error < MINERROR) { localcon++; } } //my d here is the best concensus set. if (localcon>=consize)
{
double totalerror = 0.0; consize = localcon;
for (int i = 0; i{
double ls = (globalA[i].x * cos(omega))-(globalA[i].y * sin(omega)) + Tx - localB[i].x;
double rs = (globalA[i].x * sin(omega))+(globalA[i].y * cos(omega)) + Ty - localB[i].y;
totalerror = totalerror + (ls*ls) + (rs*rs);
}
//keeps track of the perimeters which give the smallest result.
if (totalerror < bestconerror)
{
bestconerror= totalerror; model[0] = omega;
model[1] = Tx; model[2] = Ty;
}}}
//Creates a homography matrix given our best model
omega = model[0]; Tx = model[1]; Ty = model[2];
double a=cos(omega); double b=-sin(omega); double c=Tx; double d=sin(omega); double e=cos(omega); double f=Tx; double g=0.0; double h=0.0; double i=1.0;
H = (Mat_(3,3) << a, b, c, d, e, f, g, h, i);
The H matrix is then used to find where the points from local image would be on the global image. The tricky part was actually making the new global image. The secret is the fact that the local image can be on all sides of the old global image. To make this possible, a new global image was created which had a size of old_global_image + 2 * local image. The old global image was then copied to the new global image. The next step was copying the local image to the new global image, by transforming it according to the calculated transform. The next tricky part was to reduce the size of the new global image, by finding the top left and the bottom right points of the image, cropping it, and saving it on the computer.
Following images were created using this node:
INTRO-MISSION 