Point2f calcCenterOfMass(vector<Point2f> pointCloud) {
    Point2f COM = Point2f(0, 0);
    if (!pointCloud.empty()) {
        for (auto &point : pointCloud)
            COM = COM + point;
        COM = COM / (float) pointCloud.size();
    }
    return COM;
}

Point2f normalize(Point2f point){
    float magnitude = sqrt(point.x*point.x+point.y*point.y);
    return point/magnitude;
}

float calcRotation(vector<KeyPoint> matched1, vector<KeyPoint> matched2) {
    //make keypoints to Points
    vector<Point2f> pointCloudOne;
    vector<Point2f> pointCloudTwo;
    for (auto &match : matched1)
        pointCloudOne.push_back(match.pt);
    for (auto &match : matched2)
        pointCloudTwo.push_back(match.pt);
    //calc centerofmasses
    Point2f COM1 = calcCenterOfMass(pointCloudOne);
    Point2f COM2 = calcCenterOfMass(pointCloudTwo);

    //move second pointcloud to first
    Point2f difference = COM1 - COM2;
    for(auto &point: pointCloudTwo)
        point += difference;

    float angle=0;
    for(int i=0; i<pointCloudOne.size(); i++){
        Point2f p1normed = normalize(pointCloudOne.at(i));
        Point2f p2normed = normalize(pointCloudTwo.at(i));
        angle += acosf(p1normed.dot(p2normed));
    }
    return angle/pointCloudOne.size();
}