//comparer to sort vertices in ccw order relative to vertices[0] (lowest point)
bool comparerCCW(Vertex p1, Vertex p2) {
int cpVal = crossProduct(vertices[0], p1, p2);
if (cpVal > 0)
return true;//CCW
if (cpVal < 0)
return false;//CW
if (euclidDistance(vertices[0], p1) < euclidDistance(vertices[0], p2))
return true;//colinear
return false;
}
cv::Point2i findNNEuclid(const cv::Mat& img, cv::Point2i p) {
cv::Point2i nearest(0, 0);
double minDist = std::numeric_limits<double>::max();
for (int i = 0; i < img.rows/2; i+=4) {
for (int j = 0; j < img.cols; j+=4) {
double dist = euclidDistance(img, img, p, cv::Point2i(j, i), 4);
if (dist < minDist) {
minDist = dist;
nearest = cv::Point2i(j, i);
}
}
}
return nearest;
}
/** Returns the cost vector of two sets of points and usestheir euclidance distance as cost
\param pointsSet1 : first set of points
\param pointsSet2 : second set of points
\return : cost vector of two sets of points (*dist_vector)
*/
void Utility::another_hung_distance(std::vector<Point2D> pointsSet1, std::vector<Point2D> pointsSet2, int *dist_vector) // this is for the case that number of points are not same ...
{
int s1 = (int)pointsSet1.size();
int s2 = (int)pointsSet2.size();
//printf("\n s1 = %d",s1);
//printf("\n s2 = %d",s2);
//assert(s1 == s2);
for (int i = 0;i <s1;i++)
for (int j = 0;j < s2; j++)
dist_vector[s2*i + j] = (int)euclidDistance(pointsSet1[i],pointsSet2[j]);
}
cv::Point2i nearestDiff(const cv::Mat& img1, const cv::Mat& img2, cv::Point2i p) {
cv::Point2i nearest(0, 0);
cv::Mat diff(img1.size(), CV_8U);
double minDist = std::numeric_limits<double>::max();
for (int i = 0; i < img2.rows; i+=8) {
for (int j = 0; j < img2.cols; j+=8) {
double dist = euclidDistance(img1, img2, p, cv::Point2i(j, i), 8);
if (dist < minDist) {
minDist = dist;
nearest = cv::Point2i(j, i);
}
}
}
return nearest;
}
void Utility::hungCorrespondOf2Sets (std::vector<Point3D> set1, std::vector<Point3D>set2, std::vector<int>&assignment1,std::vector<int>&assignment2)
{
int nrows = set1.size();
int ncols = set2.size();
int maxSize = std::max(nrows,ncols);
assignment1.resize(maxSize);
assignment2.resize(maxSize);
for (int i = 0;i<maxSize;i++)
{
assignment1[i] = -1;
assignment2[i] = -1;
}
Matrix<double> costMatrix(maxSize, maxSize);
for (int i = 0;i<nrows;i++)
for (int j = 0;j<ncols;j++)
costMatrix(i,j) = euclidDistance(set1[i],set2[j]);
Munkres myMunkres;
myMunkres.solve(costMatrix);
// solution is back stored in costMatrix variable, 0 for matches, otherwise -1;
// now fill in assignment
for (int i = 0;i<maxSize;i++)
{
for (int j = 0;j<maxSize;j++)
{
if (costMatrix(i, j) ==0)
{
if (i<nrows && j<ncols)
{
assignment1[i] = j;
assignment2[j] = i;
}
}
}
}
}
void imKMeans(const cv::Mat& src, cv::Mat& dst) {
cv::Mat res = src.clone();
std::vector<uchar> centers[3];
std::vector<cv::Point2i> groups[3] = {
{cv::Point2i(0, 0)},
{cv::Point2i(4, 0)},
{cv::Point2i(8, 0)},
};
for (int i = 0; i < 3; i++) {
centers[i] = computeCenter(src, groups[i]);
// std::cout << "Initial groups: " << groups[i] << "\n";
}
for (int i = 0; i < src.rows/2; i+=4) {
for (int j = 0; j < src.cols; j+=4) {
if (i == 0 && j <= 8) {
continue;
}
cv::Point2i loc(j, i);
// std::cout << loc << "\n";
double minDist = std::numeric_limits<double>::max();
int nearest = 0;
for (int k = 0; k < 3; k++) {
double dist = euclidDistance(src, loc, centers[k], 4);
if (dist < minDist) {
minDist = dist;
nearest = k;
}
}
groups[nearest].push_back(loc);
// std::cout << "Updated group " << nearest << " " << groups[nearest] << std::endl;
centers[nearest] = computeCenter(src, groups[nearest]);
switch(nearest) {
case 0:
maskFill(res, j, i, 4, 0);
break;
case 1:
maskFill(res, j, i, 4, 128);
break;
case 2:
maskFill(res, j, i, 4, 255);
break;
}
}
}
dst = res;
}
/** Returns the BackProjectionError_struct when two points in different views are intersected
* (here "intersection" means the center of line with minimum distance between 3D lines joining optical center and 3D projection os these points in their respective views).
* \n Here is a step by step working of this function:
* -# Construct the 2 lines (in 3D) pasing through optical center (of their respective views) and intersecting the ViewPlane at given 2D points.
* -# Find the equation of shortest line (in 3D) joining these two lines. This would be a single point, if the lines actually intersect.
* -# Compute the center of this shortest line, and assume it to be "true" 3D point for stereo pair: "pt2D_view1" and "pt2D_view2".
* -# Now to see the validity of "true" 3D point, backProject this 3D point to each of views again, and call these 2D points as : new_pt_view1 , new_pt_view2
* -# Compute backPojection error as Manhattan distance (|x2-x1| + |y2-y1|) between original 2D points (passed as input) and backProjection of "true" 3D points. And also the
length of shortest line joining two backprojected 3D lines.
* -# Return the BackProjection_struct: Manhattan distance for View1, Manhattan Distance for View2 and Length of shortest line joining two 3D lines as the error.
(If the input 2D points, weere actually the same point, the error should be very less for all values in the structure).
* \param : pt2D_view1 2D Point (input) in View 1
* \param : pt2D_view2 2D Point (input) in View 2
* \param : *v1 Pointer to View 1, so that we can access Camera Parameters (a simple technicality).
* \param : *v2 Pointer to View 2, so that we can access Camera Parameters (a simple technicality).
* \return : BackProjectionError_struct. See the description above for details.
\sa LineLineIntersect(), backProject(), get2Dfrom3D()
\relates View
*/
BackProjectionError_struct computeBackProjectionError(Point2D pt2D_view1, Point2D pt2D_view2,View *v1,View *v2)
{
BackProjectionError_struct toReturn;
//////////////////////////////////////////////////////////////////////////////////////////////////
// Case for dummy points - added for G-correspondence
int howManyInvalid = ( (isInvalidPoint(pt2D_view1))?1:0 ) + ( (isInvalidPoint(pt2D_view2))?1:0 );
if(howManyInvalid == 2) // Both the points are dummy point, then error between them = -Infinity
{
toReturn.errV1 = -1.0*INFINITY_ALEPH0;
toReturn.errV2 = -1.0*INFINITY_ALEPH0;
toReturn.shortestLineLength = -1.0*INFINITY_ALEPH0;
return toReturn;
}
if(howManyInvalid == 1) // Exactly one of them is dummy (invalid) point, then error = +Infinity
{
toReturn.errV1 = INFINITY_ALEPH0;
toReturn.errV2 = INFINITY_ALEPH0;
toReturn.shortestLineLength = INFINITY_ALEPH0;
return toReturn;
}
// If execution reach here => None of the point is "invalid" (dummy), then proceed as usual
///////////////////////////////////////////////////////////////////////////////////
Point3D pt3D_view1 = backProject(pt2D_view1,v1->camParams.invProjMatrix); // Get an arbitrary 3D point lying on line joining optical center and intersecting View 1 plane at pt2D_view1
Point3D pt3D_view2 = backProject(pt2D_view2,v2->camParams.invProjMatrix); // Get an arbitrary 3D point lying on line joining optical center and intersecting View 2 plane at pt2D_view2
double mua,mub;
Point3D Pa,Pb;
LineLineIntersect(v1->camParams.opticalCenter,pt3D_view1,v2->camParams.opticalCenter,pt3D_view2,&Pa,&Pb,&mua,&mub); // Find the shortest distance line (Pa-Pb) among two 3D lines
Point3D center; // This is supposedly the 3D point computed from stereo pair pt2D_view1 and pt2D_view2
center = meanOfPoints(Pa,Pb); // Compute center of Pa,Pb
Point2D new_pt_view1 = get2Dfrom3D(center,v1->camParams.projMatrix); // BackProject "center" to view 1
Point2D new_pt_view2 = get2Dfrom3D(center,v2->camParams.projMatrix); // BackProject "center" to view 2
// Compute the Manhattan distances between original and backProjected 2D points
toReturn.errV1 = fabs(pt2D_view1.x-new_pt_view1.x) + fabs(pt2D_view1.y-new_pt_view1.y);
toReturn.errV2 = fabs(pt2D_view2.x-new_pt_view2.x) + fabs(pt2D_view2.y-new_pt_view2.y);
//Compute the length of shortest line joining the backprojected 3D lines.
toReturn.shortestLineLength = euclidDistance(Pa,Pb);
return toReturn;
}
bool Utility::differenceBetweenTwoGroupOfPoints( std::vector<Point2D> pnts1,std::vector<Point2D> pnts2, double &totalDist, double &maxDistofPair)
{
totalDist = 0; maxDistofPair = -1; //initialize ...
if(pnts1.size() != pnts2.size())
{
printf("\n for difference between two groups, size is not the same, so I'm returning -1");
return false; // this is for the case that we don't have same number of measurements in two consecutive frame .. so distance information is not useful
}
std::vector<int> ass1,ass2;
hungCorrespondOf2Sets(pnts1,pnts2,ass1,ass2);
for (int i = 0;i<ass1.size();i++)
{
double temp = euclidDistance(pnts1[i],pnts2[ass1[i]]);
totalDist+=temp;
if (temp>maxDistofPair)
maxDistofPair = temp;
}
return true;
}
int main (int argc, char **argv) {
int nbObjects = 5000;
int nbClasses = 20;
SparseMatrix *objectObjectSimilarities = new SparseMatrix(nbObjects, nbObjects);
// START generating test data
printf ("Generating random test data...\n");
srandom(11);
flt playFieldSize = 10;
// Randomly positioning class prototypes, no variance for now: change
// the playfield size to have classes more or less compactly packed
// together
flt *classCenters = new flt [2*nbClasses];
for (int i=0; i<nbClasses; i++) {
classCenters[2*i] = FRAND(2*playFieldSize)-playFieldSize;
classCenters[2*i+1] = FRAND(2*playFieldSize)-playFieldSize;
}
// Affecting a class to each object
int *groundTruth = new int [nbObjects];
for (int i=0; i<nbObjects; i++)
groundTruth[i] = random() % nbClasses;
// Generating object coordinates ; they follow a normal gaussian
// distribution around class centers
flt *objectVectors = new flt [2*nbObjects];
for (int i=0; i<nbObjects; i++)
twoGaussianRandoms (&(objectVectors[2*i]), &(objectVectors[2*i+1]), classCenters[2*groundTruth[i]], 1, classCenters[2*groundTruth[i]+1], 1);
printf ("Computing test data similarities...\n");
// Computing similarities between objects
flt *fullSimilarityMatrixLine = new flt[nbObjects];
for (int i=0; i<nbObjects; i++) {
if (!(i%100)) {
fprintf(stderr, "\b\b\b\b\b\b\b\b\b\b\b\b\b\b\b\b\b\b\b\b%i / %i", i, nbObjects);
}
for (int j=0; j<nbObjects; j++)
fullSimilarityMatrixLine[j] = 1. / (1. + euclidDistance (objectVectors+2*i, objectVectors+2*j, 2));
objectObjectSimilarities->setLine(i, fullSimilarityMatrixLine, .3);
}
printf ("\nSimilarity matrix density : %5.2f %%\n\n", 100*objectObjectSimilarities->getDensity());
// END generating test data
// Perform clustering test in all modes, writing out result and gnuplot commands
const char* operationMode [] = {"br", "bc", "bo", "pr", "pc", "po", "Pr", "Pc", "Po", "mr", "mc", "mo", "Mr", "Mc", "Mo"};
int *result = new int [nbObjects];
int *init = new int [nbObjects];
for (int i=0; i<nbObjects; i++)
init[i] = rand() % nbClasses;
char *filename1 = new char[128];
char *filename2 = new char[128];
for (int mode=0; mode<15; mode++) {
sprintf(filename1, "kaverages_classes_%s.txt", operationMode[mode]);
sprintf(filename2, "kaverages_classes_%s.gnuplot", operationMode[mode]);
kAveragesClustering (nbObjects, nbClasses, objectObjectSimilarities, result, operationMode[mode], 500, init);
// Create two files to allow visualizing results with gnuplot
FILE *fout = fopen(filename1, "w");
if (!fout) ERROR ("Could not open file '%s' for writing result.", filename1);
for (int c=0; c<nbClasses; c++)
fprintf(fout, "%f\t%f\n", classCenters[2*c], classCenters[2*c+1]);
fprintf(fout, "\n\n");
for (int c=0; c<nbClasses; c++) {
for (int i=0; i<nbObjects; i++) {
if (result[i] == c)
fprintf(fout, "%f\t%f\n", objectVectors[2*i], objectVectors[2*i+1]);
}
fprintf(fout, "\n\n");
}
fclose(fout);
fout = fopen(filename2, "w");
if (!fout) ERROR ("Could not open file '%s' for writing gnuplot commands.", filename2);
fprintf(fout, "unset key;\nplot \"%s\" index 0 with points", filename1);
for (int c=0; c<nbClasses; c++) {
fprintf(fout, ", \"%s\" index %i with dots", filename1, c+1);
}
fprintf(fout, ";\n");
fclose(fout);
fprintf(stderr, "\n\n----------------------------------------------\n\n");
}
delete[] classCenters;
delete[] groundTruth;
delete[] objectVectors;
delete[] fullSimilarityMatrixLine;
delete[] result;
delete objectObjectSimilarities;
}
