Adjust a detected 2D map to a reference 2D map - c++

Question

I have a map with some reference positions that correspond to the center (small cross) of some objects like this:
I take pictures to find my objects but in the pictures I have some noise so I can't always find all of the objects, it can be something like this:
From the few found positions I need to know where in the picture the other not found objects should be. I've being reading about this for the last couple of days and experimenting but I can't find a proper way of doing this. In some examples they start by calculating the center of masses and translating them together, then rotating, some other examples use least squares minimization and start by a rotation. I can't use OpenCV or any other APIs, just plain C++. I can use Eigen library if that helps. Can anyone give me some pointers on this?
EDIT:
I've solved the correspondence between points, the picture is never very different from the reference so for each found position I can search for its corresponding reference. In brief, I have one 2D matrix with reference points and another 2D matrix with found points. In the found matrix of points, the not found points are saved as NaN just to keep the same matrix size, the NaN points are not used in the calculations.

Answer 1

Since you have already matched the points to one another, finding the transform is straight forward:
Eigen::Affine2d findAffine(Eigen::Matrix2Xd const& refCloud, Eigen::Matrix2Xd const& targetCloud)
{
// get translation
auto refCom = centerOfMass(refCloud);
auto refAtOrigin = refCloud.colwise() - refCom;
auto targetCom = centerOfMass(targetCloud);
auto targetAtOrigin = targetCloud.colwise() - targetCom;
// get scale
auto scale = targetAtOrigin.rowwise().norm().sum() / refAtOrigin.rowwise().norm().sum();
// get rotation
auto covMat = refAtOrigin * targetAtOrigin.transpose();
auto svd = covMat.jacobiSvd(Eigen::ComputeFullU | Eigen::ComputeFullV);
auto rot = svd.matrixV() * svd.matrixU().transpose();
// combine the transformations
Eigen::Affine2d trans = Eigen::Affine2d::Identity();
trans.translate(targetCom).scale(scale).rotate(rot).translate(-refCom);
return trans;
}
refCloud is your reference point set and targetCloud is the set of points you have found in your image. It is important that the clouds match index wise, so refCloud[n] must be the corresponding point to targetCloud[n]. This means that you have to remove all NaNs from your matrix and cherry pick the correspondances in your reference point set.
Here is a full example. I'm using OpenCV to draw the stuff:
#include <Eigen/Dense>
#include <opencv2/opencv.hpp>
#include <vector>
#include <iostream>
using Point = Eigen::Vector2d;
template <typename TMatrix>
Point centerOfMass(TMatrix const& points)
{
return points.rowwise().sum() / points.cols();
}
Eigen::Affine2d findAffine(Eigen::Matrix2Xd const& refCloud, Eigen::Matrix2Xd const& targetCloud)
{
// get translation
auto refCom = centerOfMass(refCloud);
auto refAtOrigin = refCloud.colwise() - refCom;
auto targetCom = centerOfMass(targetCloud);
auto targetAtOrigin = targetCloud.colwise() - targetCom;
// get scale
auto scale = targetAtOrigin.rowwise().norm().sum() / refAtOrigin.rowwise().norm().sum();
// get rotation
auto covMat = refAtOrigin * targetAtOrigin.transpose();
auto svd = covMat.jacobiSvd(Eigen::ComputeFullU | Eigen::ComputeFullV);
auto rot = svd.matrixV() * svd.matrixU().transpose();
// combine the transformations
Eigen::Affine2d trans = Eigen::Affine2d::Identity();
trans.translate(targetCom).scale(scale).rotate(rot).translate(-refCom);
return trans;
}
void drawCloud(cv::Mat& img, Eigen::Matrix2Xd const& cloud, Point const& origin, Point const& scale, cv::Scalar const& color, int thickness = cv::FILLED)
{
for (int c = 0; c < cloud.cols(); c++)
{
auto p = origin + cloud.col(c).cwiseProduct(scale);
cv::circle(img, {int(p.x()), int(p.y())}, 5, color, thickness, cv::LINE_AA);
}
}
int main()
{
// generate sample reference
std::vector<Point> points = {{4, 9}, {4, 4}, {6, 9}, {6, 4}, {8, 9}, {8, 4}, {10, 9}, {10, 4}, {12, 9}, {12, 4}};
Eigen::Matrix2Xd fullRefCloud(2, points.size());
for (int i = 0; i < points.size(); i++)
fullRefCloud.col(i) = points[i];
// generate sample target
Eigen::Matrix2Xd refCloud = fullRefCloud.leftCols(fullRefCloud.cols() * 0.6);
Eigen::Affine2d refTransformation = Eigen::Affine2d::Identity();
refTransformation.translate(Point(8, -4)).rotate(4.3).translate(-centerOfMass(refCloud)).scale(1.5);
Eigen::Matrix2Xd targetCloud = refTransformation * refCloud;
// find the transformation
auto transform = findAffine(refCloud, targetCloud);
std::cout << "Original: \n" << refTransformation.matrix() << "\n\nComputed: \n" << transform.matrix() << "\n";
// apply the computed transformation
Eigen::Matrix2Xd queryCloud = fullRefCloud.rightCols(fullRefCloud.cols() - refCloud.cols());
queryCloud = transform * queryCloud;
// draw it
Point scale = {15, 15}, origin = {100, 300};
cv::Mat img(600, 600, CV_8UC3);
cv::line(img, {0, int(origin.y())}, {800, int(origin.y())}, {});
cv::line(img, {int(origin.x()), 0}, {int(origin.x()), 800}, {});
drawCloud(img, refCloud, origin, scale, {0, 255, 0});
drawCloud(img, fullRefCloud, origin, scale, {255, 0, 0}, 1);
drawCloud(img, targetCloud, origin, scale, {0, 0, 255});
drawCloud(img, queryCloud, origin, scale, {255, 0, 255}, 1);
cv::flip(img, img, 0);
cv::imshow("img", img);
cv::waitKey();
return 0;
}

Answer 2

I managed to make it work with the code from here:
https://github.com/oleg-alexandrov/projects/blob/master/eigen/Kabsch.cpp
I'm calling the Find3DAffineTransform function and passing it my 2D maps, as this function expects 3D maps I've made all z coordinates = 0 and it works. If I have some time I'll try to adapt it to 2D.
Meanwhile a fellow programmer (Regis :-) found also this, that should work:
https://eigen.tuxfamily.org/dox/group__Geometry__Module.html#gab3f5a82a24490b936f8694cf8fef8e60
Its the function umeyama() that returns the transformation between two point sets. Its part of Eigen library. Didn't have the time to test this.