$search

reconstruction.cpp

Go to the documentation of this file.

#include "reconstruction.hpp"

void summarizeTransformation(const Mat& C, char *summary) {

        Mat P, R, Rv, t;
        transformationToProjection(C, P);
        decomposeTransform(C, R, t);
        Rodrigues(R, Rv);
        
        double unitsDist, degreesRot;
        unitsDist = getDistanceInUnits(t);
        degreesRot = getRotationInDegrees(R);
        
        sprintf(summary, "%f units (%f, %f, %f); %f degrees", unitsDist, t.at<double>(0,0), t.at<double>(1,0), t.at<double>(2,0), degreesRot);
        
}

int findBestCandidate(const Mat *CX, const Mat& K, const vector<Point2f>& pts1, const vector<Point2f>& pts2, Mat& C) {
        
        int bestScore = 0, bestCandidate = 0;

        for (int kkk = 0; kkk < 4; kkk++) {

                int validPts = pointsInFront(CX[kkk], K, pts1, pts2);
                
                if (validPts > bestScore) {
                        bestScore = validPts;
                        bestCandidate = kkk;
                }
                
                //printf("%s << validPts(%d) = %d\n", __FUNCTION__, kkk, validPts);
        }
        
        CX[bestCandidate].copyTo(C);
        
        return bestScore;
}

void reverseTranslation(Mat& C) {
        
        for (unsigned int iii = 0; iii < 3; iii++) {
                C.at<double>(iii,3) = -C.at<double>(iii,3);
        }
        
}



void findFeaturesForPoints(vector<featureTrack>& tracks, vector<Point2f>& pts1, vector<Point2f>& pts2, vector<Point3d>& pts3d, int idx1, int idx2) {

        // For each 3d point, find the track that contains it, then find the projections...
        for (unsigned int jjj = 0; jjj < pts3d.size(); jjj++) {
                
                //printf("%s << jjj = %d (%f, %f, %f)\n", __FUNCTION__, jjj, pts3d.at(jjj).x, pts3d.at(jjj).y, pts3d.at(jjj).z);
                
                for (unsigned int iii = 0; iii < tracks.size(); iii++) {

                        //printf("%s << iii = %d\n", __FUNCTION__, iii);
                        
                        // MONKEY!!
                        if (tracks.at(iii).get3dLoc() == pts3d.at(jjj)) {
                                
                                //printf("%s << MATCH! (%d, %d)\n", __FUNCTION__, jjj, iii);
                                
                                for (unsigned int kkk = 0; kkk < tracks.at(iii).locations.size(); kkk++) {
                                        
                                        //printf("%s << kkk = %d (%d, %d)\n", __FUNCTION__, kkk, tracks.at(iii).locations.at(kkk).imageIndex, tracks.at(iii).locations.at(kkk).imageIndex);
                        
                                        if (tracks.at(iii).locations.at(kkk).imageIndex == idx1) {
                                                
                                                pts1.push_back(tracks.at(iii).locations.at(kkk).featureCoord);
                                                
                                        } else if (tracks.at(iii).locations.at(kkk).imageIndex == idx2) {
                                                
                                                pts2.push_back(tracks.at(iii).locations.at(kkk).featureCoord);
                                                
                                        }
                                        
                                }
                                
                        }
                        
                }
                
                
        }
        
}

int countActiveTriangulatedTracks(vector<unsigned int>& indices, vector<featureTrack>& tracks) {
        
        int triangulatedCount = 0;
        
        for (unsigned int iii = 0; iii < indices.size(); iii++) {
                
                if (tracks.at(indices.at(iii)).isTriangulated) {
                        triangulatedCount++;
                }
                
        }
        
        return triangulatedCount;
        
}

void getIndicesForTriangulation(vector<unsigned int>& dst, vector<unsigned int>& src, vector<unsigned int>& already_triangulated) {
        
        dst.clear();
        
        for (unsigned int iii = 0; iii < src.size(); iii++) {
                
                bool triangulated = false;
                
                for (unsigned int jjj = 0; jjj < already_triangulated.size(); jjj++) {
                        
                        if (already_triangulated.at(jjj) == src.at(iii)) {
                                triangulated = true;
                                continue;
                        }

                        
                }
                
                if (!triangulated) {
                        
                        dst.push_back(src.at(iii));
                        
                }

                
        }
        
}

void reconstructSubsequence(vector<featureTrack>& tracks, vector<Point3d>& ptCloud, int idx1, int idx2) {
        
}

void removeShortTracks(vector<featureTrack>& tracks, int idx1, int idx2) {
        
        int minLength = (idx2 - idx1) / 2;
        
        for (unsigned int iii = 0; iii < tracks.size(); iii++) {
                int trackLength = tracks.at(iii).locations.size();
                if (trackLength < minLength) {
                        int final_image = tracks.at(iii).locations.at(trackLength-1).imageIndex;
                        
                        if ((final_image >= idx1) && (final_image < idx2)) {
                                tracks.erase(tracks.begin() + iii);
                                iii--;
                        }
                        
                }
        }
        
}

void updateSystemTracks(SysSBA& sys, vector<featureTrack>& tracks, unsigned int start_index) {
        
        //sys = SysSBA();
        
        sys.tracks.clear();

        
        for (unsigned int iii = 0; iii < tracks.size(); iii++) {
                
                //printf("%s << Checking track (%d) (size = %d)\n", __FUNCTION__, iii, tracks.at(iii).locations.size());
                
                if (tracks.at(iii).isTriangulated) {

                        
                        //printf("%s << Track is triangulated\n", __FUNCTION__);
                        
                        Vector4d temppoint(tracks.at(iii).get3dLoc().x, tracks.at(iii).get3dLoc().y, tracks.at(iii).get3dLoc().z, 1.0);
                        sys.addPoint(temppoint);
                        
                        //printf("%s << Point added; nodes = %d\n", __FUNCTION__, sys.nodes.size());
                        //printf("%s << sys.tracks.size() = %d\n", __FUNCTION__, sys.tracks.size());
                        
                        // sys.tracks.size()-1
                        
        
                        for (unsigned int jjj = 0; jjj < tracks.at(iii).locations.size(); jjj++) {
                                
                                // Only want to add projections for active nodes
                                
                                if ((((unsigned int) tracks.at(iii).locations.at(jjj).imageIndex) >= start_index) && (((unsigned int) tracks.at(iii).locations.at(jjj).imageIndex) < (start_index+sys.nodes.size()))) {
                                        //printf("%s << Assembling projection (%d)(%d)\n", __FUNCTION__, iii, jjj);
                                
                                        Vector2d proj;
                                        
                                        proj.x() = tracks.at(iii).locations.at(jjj).featureCoord.x;
                                        proj.y() = tracks.at(iii).locations.at(jjj).featureCoord.y;
                                        
                                        //printf("%s << Adding projection (track: %d)(loc: %d) node #: [%d]\n", __FUNCTION__, iii, jjj, tracks.at(iii).locations.at(jjj).imageIndex);
                                        
                                        //printf("%s << vec size (tracks: %d, nodes for this track: %d)\n", __FUNCTION__, tracks.size(), tracks.at(iii).locations.size());
                                        //printf("%s << sys size (tracks: %d, nodes: %d)\n", __FUNCTION__, sys.tracks.size(), sys.nodes.size());
                                        
                                        // If you only have 30 nodes, then how are you adding a track that has an image index of 53?
                                        
                                        //printf("%s << Adding mono projection: (cam: %d / %d, track: %d / %d) = (%f, %f)\n", __FUNCTION__, tracks.at(iii).locations.at(jjj).imageIndex, sys.nodes.size(), sys.tracks.size()-1, sys.tracks.size(), proj.x(), proj.y());
                                        sys.addMonoProj(tracks.at(iii).locations.at(jjj).imageIndex-start_index, sys.tracks.size()-1, proj);
                                        
                                        //printf("%s << Added.\n", __FUNCTION__);
                                        
                                        // 0, 53, proj
                                }
                                
                                
                                
                        }
                        
                        //printf("%s << Projections added\n", __FUNCTION__);
                
                }
                
        }
        
}

void updateTriangulatedPoints(vector<featureTrack>& tracks, vector<unsigned int>& indices, vector<Point3d>& cloud) {
        
        if (cloud.size() != indices.size()) {
                printf("%s << ERROR! Cloud size and index list size don't match (%d vd %d)\n", __FUNCTION__, cloud.size(), indices.size());
        }
        
        for (unsigned int iii = 0; iii < indices.size(); iii++) {
                
                if (tracks.at(indices.at(iii)).isTriangulated) {
                        //printf("%s << (%d) is triangulated; moving from (%f, %f, %f) to (%f, %f, %f)\n", __FUNCTION__, iii, tracks.at(indices.at(iii)).get3dLoc().x, tracks.at(indices.at(iii)).get3dLoc().y, tracks.at(indices.at(iii)).get3dLoc().z, cloud.at(iii).x, cloud.at(iii).y, cloud.at(iii).z);
                }
                
                tracks.at(indices.at(iii)).set3dLoc(cloud.at(iii));
                
                
                
        }
        
}

void reduceActiveToTriangulated(vector<featureTrack>& tracks, vector<unsigned int>& indices, vector<unsigned int>& untriangulated) {
        
        for (unsigned int iii = 0; iii < indices.size(); iii++) {
                
                if (!tracks.at(indices.at(iii)).isTriangulated) {
                        untriangulated.push_back(indices.at(iii));
                        indices.erase(indices.begin() + iii);
                        iii--;
                }
                
        }
        
}

void filterToActivePoints(vector<featureTrack>& tracks, vector<Point2f>& pts1, vector<Point2f>& pts2, vector<unsigned int>& indices, int idx1, int idx2) {

        pts1.clear();
        pts2.clear();
        
        for (unsigned int iii = 0; iii < indices.size(); iii++) {
                
                for (unsigned int jjj = 0; jjj < tracks.at(indices.at(iii)).locations.size()-1; jjj++) {
                        
                        if (tracks.at(indices.at(iii)).locations.at(jjj).imageIndex == idx1) {
                                
                                pts1.push_back(tracks.at(indices.at(iii)).locations.at(jjj).featureCoord);
                                
                                for (unsigned int kkk = jjj+1; kkk < tracks.at(indices.at(iii)).locations.size(); kkk++) {
                                        
                                        if (tracks.at(indices.at(iii)).locations.at(kkk).imageIndex == idx2) {
                                                
                                                pts2.push_back(tracks.at(indices.at(iii)).locations.at(kkk).featureCoord);
                                                
                                                continue;
                                        }
                                        
                                }
                                
                                continue;
                                
                        }
                        
                }

        }
        
        
}

void getActive3dPoints(vector<featureTrack>& tracks, vector<unsigned int>& indices, vector<Point3d>& cloud) {
        
        cloud.clear();
        
        for (unsigned int iii = 0; iii < indices.size(); iii++) {
                cloud.push_back(tracks.at(indices.at(iii)).get3dLoc());
        }
        
}

void filterToCompleteTracks(vector<unsigned int>& dst, vector<unsigned int>& src, vector<featureTrack>& tracks, int idx1, int idx2) {
        
        dst.clear();
        
        for (unsigned int iii = 0; iii < src.size(); iii++) {
                
                if (tracks.at(src.at(iii)).locations.size() >= (idx2 - idx1)) {
                        
                        for (unsigned int jjj = 0; jjj < tracks.at(src.at(iii)).locations.size()-(idx2-idx1); jjj++) {
                                
                                //printf("%s << DEBUG [%d][%d]\n", __FUNCTION__, iii, jjj);
                        
                                if (tracks.at(src.at(iii)).locations.at(jjj).imageIndex == idx1) {
                                        
                                        //printf("%s << DEBUG [%d][%d] X\n", __FUNCTION__, iii, jjj);
                                        
                                        for (unsigned int kkk = jjj+1; kkk < tracks.at(src.at(iii)).locations.size(); kkk++) {
                                                
                                                //printf("%s << DEBUG [%d][%d][%d]\n", __FUNCTION__, iii, jjj, kkk);
                                                
                                                if (tracks.at(src.at(iii)).locations.at(kkk).imageIndex == idx2) {
                                                        
                                                        //printf("%s << DEBUG [%d][%d][%d] X\n", __FUNCTION__, iii, jjj, kkk);
                                                        
                                                        dst.push_back(src.at(iii));
                                                        
                                                }
                                                
                                        }
                                        
                                }
                                
                                
                        }
                        
                }
                
        }
        
}

void getActiveTracks(vector<unsigned int>& indices, vector<featureTrack>& tracks, int idx1, int idx2) {
        
        printf("%s << Entered.\n", __FUNCTION__);
        
        // Should only consider a track active if it contains at least two projections in the subsequence
        
        indices.clear();
        
        for (unsigned int iii = 0; iii < tracks.size(); iii++) {
                
                //printf("%s << DEBUG [%d]\n", __FUNCTION__, iii);
                
                if (tracks.at(iii).locations.size() < 2) {
                        continue;
                }
                
                for (unsigned int jjj = 0; jjj < tracks.at(iii).locations.size()-1; jjj++) {
                        
                        //printf("%s << DEBUG [%d][%d]\n", __FUNCTION__, iii, jjj);
                
                        if ((tracks.at(iii).locations.at(jjj).imageIndex >= idx1) && (tracks.at(iii).locations.at(jjj).imageIndex <= idx2)) {
                                
                                for (unsigned int kkk = jjj+1; kkk < tracks.at(iii).locations.size(); kkk++) {
                                        
                                        if ((tracks.at(iii).locations.at(kkk).imageIndex >= idx1) && (tracks.at(iii).locations.at(kkk).imageIndex <= idx2)) {
                                                indices.push_back(iii);
                                                break;
                                        }
                                        
                                }
                                
                                break;
                        }
                        
                        
                }
                
                
                
                
                
        }
        
        printf("%s << Exiting.\n", __FUNCTION__);
        
}

bool estimatePoseFromKnownPoints(Mat& dst, cameraParameters camData, vector<featureTrack>& tracks, unsigned int index, const Mat& guide) {
        
        vector<Point3f> points_3d;
        vector<Point2f> points_2d;
        
        for (unsigned int iii = 0; iii < tracks.size(); iii++) {
                
                //printf("%s << DEBUG [%d]\n", __FUNCTION__, iii);
                
                for (unsigned int jjj = 0; jjj < tracks.at(iii).locations.size(); jjj++) {
                        
                        //printf("%s << DEBUG [%d][%d]\n", __FUNCTION__, iii, jjj);
                
                        if (tracks.at(iii).locations.at(jjj).imageIndex == index) {
                                
                                /*
                                if ((index >= 224) && (index <= 226)) {
                                        printf("%s << Found track (%d) in image (%d)\n", __FUNCTION__, iii, index);
                                }
                                */
                                
                                if (tracks.at(iii).isTriangulated) {
                                        points_2d.push_back(tracks.at(iii).locations.at(jjj).featureCoord);
                                        Point3f tmp_pt = Point3f(((float) tracks.at(iii).get3dLoc().x), ((float) tracks.at(iii).get3dLoc().y), ((float) tracks.at(iii).get3dLoc().z));
                                        points_3d.push_back(tmp_pt);
                                        
                                }


                                break;
                        }
                        
                        
                }

                
        }
        
        Mat Rvec, R, t;
        bool guided = false;
        
        if (guide.rows > 0) {
                decomposeTransform(guide, R, t);
                Rodrigues(R, Rvec);
                guided = true;
        }
        
        Mat inlierPts;
        
        
        if (points_2d.size() < 8) {
                printf("%s << Insufficient pts (%d) in frame (%d)\n", __FUNCTION__, points_2d.size(), index);
                guide.copyTo(dst);
                
                //while (1) {}
                return false;
        }
        
        //printf("%s << About to solve PnP to estimate pose; (%d) pts\n", __FUNCTION__, points_2d.size());
        
        // ORIGINAL
        //solvePnPRansac(points_3d, points_2d, camData.K, camData.blankCoeffs, Rvec, t, guided, 300, 6.0, ((unsigned int) ((double) points_2d.size()) * 0.9), inlierPts);
        
        // DIDNT WORK
        //solvePnPRansac(points_3d, points_2d, camData.K, camData.blankCoeffs, Rvec, t, guided, 100, 8.0, ((unsigned int) ((double) points_2d.size()) * 0.9), inlierPts);
        
        // WORKED WELL!
        // solvePnPRansac(points_3d, points_2d, camData.K, camData.blankCoeffs, Rvec, t, guided, 100, 6.0, ((unsigned int) ((double) points_2d.size()) * 0.9), inlierPts);
        
        // SEEMS TO WORK...
        //solvePnPRansac(points_3d, points_2d, camData.K, camData.blankCoeffs, Rvec, t, guided, 100, 5.0, ((unsigned int) ((double) points_2d.size()) * 0.9), inlierPts);
        
        solvePnPRansac(points_3d, points_2d, camData.K, camData.blankCoeffs, Rvec, t, guided, 100, 4.0, ((unsigned int) ((double) points_2d.size()) * 0.9), inlierPts);
        
        Rodrigues(Rvec, R);
        
        Mat P;
        
        findP1Matrix(P, R, t);
                                                
        projectionToTransformation(P, dst);
        
        dst = dst.inv();
        
        return true;
        
}



void getCorrespondingPoints(vector<featureTrack>& tracks, const vector<Point2f>& pts1, vector<Point2f>& pts2, int idx0, int idx1) {
        
        printf("%s << Entered X.\n", __FUNCTION__);
        
        // For each track
        for (unsigned int iii = 0; iii < tracks.size(); iii++) {
                
                if (tracks.at(iii).locations.size() < 2) {
                        continue;
                }
                //printf("%s << Debug (%d)\n", __FUNCTION__, iii);
                // For each projection in the track
                for (unsigned int kkk = 0; kkk < tracks.at(iii).locations.size()-1; kkk++) {
                
                        //printf("%s << Debug (%d)(%d)\n", __FUNCTION__, iii, kkk);
                        
                        // If the camera of the projection matches the aim (idx0)
                        if (tracks.at(iii).locations.at(kkk).imageIndex == idx0) {
                                // Found a feature that exists in the first image
                                
                                //printf("%s << Debug (%d)(%d) 2\n", __FUNCTION__, iii, kkk);
                                
                                // For each of the first set of points
                                for (unsigned int jjj = 0; jjj < pts1.size(); jjj++) {
                                
                                        //printf("%s << Debug (%d)(%d)(%d)\n", __FUNCTION__, iii, kkk, jjj);
                                
                                        // If the projection location matches the first point at this point 
                                        if ((tracks.at(iii).locations.at(kkk).featureCoord.x == pts1.at(jjj).x) && (tracks.at(iii).locations.at(kkk).featureCoord.y == pts1.at(jjj).y)) {
                                                
                                                //printf("%s << Debug (%d)(%d)(%d) 2\n", __FUNCTION__, iii, kkk, jjj);
                                                
                                                // For each of the later projections on that track
                                                for (unsigned int ppp = kkk; ppp < tracks.at(iii).locations.size(); ppp++) {
                                                        
                                                        //printf("%s << Debug (%d)(%d)(%d)(%d)\n", __FUNCTION__, iii, kkk, jjj, ppp);
                                                        
                                                        // If this later projection matches the second aimed index
                                                        if (tracks.at(iii).locations.at(ppp).imageIndex == idx1) {
                                                                
                                                                //printf("%s << Debug (%d)(%d)(%d)(%d) 2\n", __FUNCTION__, iii, kkk, jjj, ppp);
                                                                
                                                                // If the first projec....?
                                                                if (jjj != pts2.size()) {
                                                                        printf("%s << ERROR! Vectors are not syncing (jjj = %d), pts2.size() = %d\n", __FUNCTION__, jjj, pts2.size());
                                                                } else {
                                                                        pts2.push_back(tracks.at(iii).locations.at(ppp).featureCoord);
                                                                }

                                                                
                                                        }
        
                                                        
                                                }
                                                
                                                break;
                                        }
                                        
                                }
                                
                                
                                break;
                        }
                        
                }
        }
                
        printf("%s << Exiting.\n", __FUNCTION__);
        
}

void getTriangulatedFullSpanPoints(vector<featureTrack>& tracks, vector<Point2f>& pts1, vector<Point2f>& pts2, int idx1, int idx2, vector<Point3f>& points3) {
        
        pts1.clear();
        pts2.clear();
        points3.clear();
        
        for (unsigned int iii = 0; iii < tracks.size(); iii++) {
                
                for (unsigned int kkk = 0; kkk < tracks.at(iii).locations.size()-1; kkk++) {
                        
                        // If the track extends between the two images of interest
                        if (tracks.at(iii).locations.at(kkk).imageIndex == idx1) {
                                
                                for (unsigned int jjj = kkk+1; jjj < tracks.at(iii).locations.size(); jjj++) {
                                        
                                        if (tracks.at(iii).locations.at(jjj).imageIndex == idx2) {
                                                
                                                if (tracks.at(iii).isTriangulated) {
                                                        
                                                        pts1.push_back(tracks.at(iii).locations.at(kkk).featureCoord);
                                                        pts2.push_back(tracks.at(iii).locations.at(jjj).featureCoord);
                                                        
                                                        points3.push_back(Point3f(((float) tracks.at(iii).get3dLoc().x), ((float) tracks.at(iii).get3dLoc().y), ((float) tracks.at(iii).get3dLoc().z)));
                                                        
                                                        break;
                                                }
                                                
                                                
                                                
                                        }
                                }
                                
                        }
                }
        }
        
}

void findTriangulatableTracks(vector<featureTrack>& tracks, vector<unsigned int>& indices, unsigned int latest_index, unsigned int min_length) {
        
        //int earliest_index = ((int) latest_index) - ((int) min_length);
        
        /*
        if (earliest_index < 0) {
                return;
        }
        */
        
        //printf("%s << Searching up to camera (%d)\n", __FUNCTION__, latest_index);
        
        bool trackAdded;
        
        for (unsigned int iii = 0; iii < tracks.size(); iii++) {
                
                trackAdded = false;
                
                if (tracks.at(iii).locations.size() > 0) {
                        //printf("%s << Track (%d) spans from (%d) to (%d) and is triangulated? (%d)\n", __FUNCTION__, iii, tracks.at(iii).locations.at(0).imageIndex, tracks.at(iii).locations.at(tracks.at(iii).locations.size()-1).imageIndex, tracks.at(iii).isTriangulated);
                } else {
                        continue;
                }
                
                
                if ((!tracks.at(iii).isTriangulated) && (tracks.at(iii).locations.size() >= min_length)) {
                        
                        unsigned int appearanceCount = 0;
                        
                        for (unsigned int jjj = 0; jjj < tracks.at(iii).locations.size(); jjj++) {
                                if (tracks.at(iii).locations.at(jjj).imageIndex <= latest_index) {
                                        appearanceCount++;
                                        
                                        
                                }
                                

                                if (appearanceCount >= min_length) {
                                        indices.push_back(iii);
                                        break;
                                }
                        }
                        
                        /*
                        for (unsigned int jjj = 0; jjj < tracks.at(iii).locations.size()-min_length; jjj++) {
                                if (tracks.at(iii).locations.at(jjj).imageIndex == earliest_index) {
                                        
                                        for (unsigned int kkk = jjj+min_length-1; kkk < tracks.at(iii).locations.size(); kkk++) {
                                                
                                                if (tracks.at(iii).locations.at(kkk).imageIndex == latest_index) {
                                                        indices.push_back(iii);
                                                        trackAdded = true;
                                                        break;
                                                }
                                                
                                        }
                                        
                                }
                                
                                if (trackAdded) {
                                        break;
                                }
                        }
                        * */
                        
                        //printf("%s << And (%d) was not added...\n", __FUNCTION__, iii);
                }
        }
        
}

void getTranslationBetweenCameras(Mat& C1, Mat& C2, double *translations) {
        
        Mat CD = C2 - C1;
        
        translations[0] = CD.at<double>(0,3);
        translations[1] = CD.at<double>(1,3);
        translations[2] = CD.at<double>(2,3);

}

void triangulateTracks(vector<featureTrack>& tracks, vector<unsigned int>& indices, cameraParameters& cameraData, Mat *cameras, unsigned int earliest_index, unsigned int latest_index) {
        
        //return;
        
        //printf("%s << Entered.\n", __FUNCTION__);
        
        unsigned int minPairs = 10;
        
        // Should first check through camera pairs to see which ones can achive valid triangulations:
        
        //printf("%s << latest_index/earliest_index = (%d/%d)\n", __FUNCTION__, latest_index, earliest_index);
        unsigned int pair_width = latest_index - earliest_index + 1;
        Mat validFramePairs = Mat::zeros(pair_width, pair_width, CV_8UC1);
        
        double translations[3];
        
        for (unsigned int iii = earliest_index; iii < latest_index; iii++) {
                for (unsigned int jjj = iii+1; jjj <= latest_index; jjj++) {
                        
                        if (cameras[iii].rows != 4) {
                                continue;
                        }
                        
                        if (cameras[jjj].rows != 4) {
                                continue;
                        }
                        
                        getTranslationBetweenCameras(cameras[iii], cameras[jjj], translations);
                        
                        //printf("%s << translations = (%f, %f, %f)\n", __FUNCTION__, translations[0], translations[1], translations[2]);
                        
                        //if ((abs(translations[0]) + abs(translations[1])) < 2.0*abs(translations[2])) {
                                
                        
                                
                        // Conditions for validity as a camera pair
                        if ((abs(translations[0] > 0.2)) || (abs(translations[1] > 0.2))) {     //  || (abs(translations[2] > 1.0))
                                validFramePairs.at<unsigned char>(iii-earliest_index,jjj-earliest_index) = 1;
                        }
                }
        }
                
        unsigned int validCount = countNonZero(validFramePairs);
        
        //printf("%s << validCount = %d\n", __FUNCTION__, validCount);
        
        if (validCount < minPairs) {
                return;
        }
        
        //printf("%s << Continuing.\n", __FUNCTION__);
        
        vector<Point3d> estimatedLocations;
        
        unsigned int contribCount = 0;
        Point3d pt3d(0.0, 0.0, 0.0);
        Point3d mean3d(0.0, 0.0, 0.0);
        Point3d stddev3d(0.0, 0.0, 0.0);
        
        
        
        for (unsigned int iii = 0; iii < indices.size(); iii++) {
                
                //printf("%s << Track (%d) ...\n", __FUNCTION__, indices.at(iii));
                
                if (tracks.size() <= indices.at(iii)) {
                        return;
                }
                
                if (tracks.at(indices.at(iii)).locations.size() < 2) {
                        continue;
                }
                
                if (tracks.at(indices.at(iii)).isTriangulated) {
                        continue;
                }
                
                //printf("%s << Track (%d) DEBUG [%d]\n", __FUNCTION__, indices.at(iii), 0);
                
                estimatedLocations.clear();
                        
                contribCount = 0;
                pt3d = Point3d(0.0, 0.0, 0.0);
                mean3d = Point3d(0.0, 0.0, 0.0);
                stddev3d = Point3d(0.0, 0.0, 0.0);
                
                unsigned int rrr, sss;
                
                for (unsigned int jjj = 0; jjj < tracks.at(indices.at(iii)).locations.size()-1; jjj++) {
                        
                        //printf("%s << Track (%d) jjj [%d]\n", __FUNCTION__, indices.at(iii), jjj);
                        
                        int rrr = tracks.at(indices.at(iii)).locations.at(jjj).imageIndex;
                        
                        if ((rrr >= earliest_index) && (rrr <= latest_index)) {
                                
                                //printf("%s << Track (%d) jjj [%d] debug [%d]\n", __FUNCTION__, indices.at(iii), jjj, 0);
                        
                                if (cameras[rrr].rows != 4) {
                                        //printf("%s << Breaking rrr (%d)...\n", __FUNCTION__, rrr);
                                        continue;
                                }
                                
                                //printf("%s << Track (%d) jjj [%d] debug [%d]\n", __FUNCTION__, indices.at(iii), jjj, 1);
                        
                                //printf("%s << rrr = %d\n", __FUNCTION__, rrr);
                                
                                Mat temp_C0;
                                cameras[rrr].copyTo(temp_C0);
                                
                                for (unsigned int kkk = jjj+1; kkk < tracks.at(indices.at(iii)).locations.size(); kkk++) {
                                        
                                        //printf("%s << Track (%d) kkk [%d]\n", __FUNCTION__, indices.at(iii), kkk);
                                        
                                        sss = tracks.at(indices.at(iii)).locations.at(kkk).imageIndex;
                                                                                
                                        if ((sss >= earliest_index) && (sss <= latest_index)) {
                                                
                                                //printf("%s << Track (%d) kkk [%d] debug [%d]\n", __FUNCTION__, indices.at(iii), kkk, 0);
                                                
                                                //printf("%s << testing
                                                
                                                if (validFramePairs.at<unsigned char>(rrr-earliest_index,sss-earliest_index) == 0) {
                                                        continue;
                                                }
                                        
                                                if (cameras[sss].rows != 4) {
                                                        //printf("%s << Breaking sss (%d)...\n", __FUNCTION__, sss);
                                                        continue;
                                                }
                                                
                                                //printf("%s << Track (%d) kkk [%d] debug [%d]\n", __FUNCTION__, indices.at(iii), kkk, 1);

                                                
                                                Mat temp_C1;
                                                cameras[sss].copyTo(temp_C1);
                                                
                                                // This should be a valid pair of projections to triangulate from for this point..
                                                
                                                // Check that the projections have enough translation:
                                                
                                                
                                                        
                                                //}
                                                
                                                Point2f pt1_, pt2_;
                                                Point3d pt3d_;
                                                
                                                pt1_ = tracks.at(indices.at(iii)).getCoord(rrr);
                                                pt2_ = tracks.at(indices.at(iii)).getCoord(sss);
                                                
                                                Triangulate(pt1_, pt2_, cameraData.K, cameraData.K_inv, temp_C1, temp_C0, pt3d_, false);
                                                
                                                //if ((pointIsInFront(temp_C0, pt3d_)) && (pointIsInFront(temp_C1, pt3d_))) {
                                                        
                                                        estimatedLocations.push_back(pt3d_);
                                                        
                                                        /*
                                                        pt3d.x += pt3d_.x;
                                                        pt3d.y += pt3d_.y;
                                                        pt3d.z += pt3d_.z;
                                                        contribCount++;
                                                        */
                                                //}
                                                
                                                if (jjj == 0) {
                                                        //printf("%s << Adding triangulation: (%f, %f, %f)\n", __FUNCTION__, pt3d_.x, pt3d_.y, pt3d_.z);
                                                }
                                        }
                                }
                        }
                        
                }
                
                //printf("%s << Track (%d) : estimatedLocations.size() = %d\n", __FUNCTION__, indices.at(iii), estimatedLocations.size());
                
                if (estimatedLocations.size() >= minPairs) {
                                
                        //printf("%s << Assigning (%d)\n", __FUNCTION__, indices.at(iii));
                        
                        //printf("%s << Calculating means..\n", __FUNCTION__);
                        
                        for (unsigned int qqq = 0; qqq < estimatedLocations.size(); qqq++) {
                                
                                mean3d.x += (estimatedLocations.at(qqq).x / ((double) estimatedLocations.size()));
                                mean3d.y += (estimatedLocations.at(qqq).y / ((double) estimatedLocations.size()));
                                mean3d.z += (estimatedLocations.at(qqq).z / ((double) estimatedLocations.size()));
                                
                        }
                        
                        //printf("%s << Calculating deviations..\n", __FUNCTION__);
                        
                        for (unsigned int qqq = 0; qqq < estimatedLocations.size(); qqq++) {
                                
                                stddev3d.x += (pow((estimatedLocations.at(qqq).x - mean3d.x), 2.0) / ((double) estimatedLocations.size()));
                                stddev3d.y += (pow((estimatedLocations.at(qqq).y - mean3d.y), 2.0) / ((double) estimatedLocations.size()));
                                stddev3d.z += (pow((estimatedLocations.at(qqq).z - mean3d.z), 2.0) / ((double) estimatedLocations.size()));
                                
                        }
                        
                        stddev3d.x = pow(stddev3d.x, 0.5);
                        stddev3d.y = pow(stddev3d.y, 0.5);
                        stddev3d.z = pow(stddev3d.z, 0.5);
                        
                        //printf("%s << Erasing outliers..\n", __FUNCTION__);
                        
                        for (int qqq = estimatedLocations.size()-1; qqq >= 0; qqq--) {
                                
                                //printf("%s << qqq = %d\n", __FUNCTION__, qqq);
                                
                                double abs_diff_x = abs(estimatedLocations.at(qqq).x - mean3d.x);
                                double abs_diff_y = abs(estimatedLocations.at(qqq).y - mean3d.y); 
                                double abs_diff_z = abs(estimatedLocations.at(qqq).z - mean3d.z); 
                                
                                if ((abs_diff_x > 2*stddev3d.x) || (abs_diff_y > 2*stddev3d.y) || (abs_diff_z > 2*stddev3d.z)) {
                                        estimatedLocations.erase(estimatedLocations.begin() + qqq);
                                }

                        }

                                
                        //printf("%s << Mean: (%f, %f, %f)\n", __FUNCTION__, mean3d.x, mean3d.y, mean3d.z);
                        //printf("%s << Std deviation: (%f, %f, %f)\n", __FUNCTION__, stddev3d.x, stddev3d.y, stddev3d.z);
                        
                        
                        if (estimatedLocations.size() >= minPairs) {
                                //printf("%s << Re-calculating mean..\n", __FUNCTION__);
                        
                                for (int qqq = 0; qqq < estimatedLocations.size(); qqq++) {
                                        
                                        //printf("%s << qqq = %d\n", __FUNCTION__, qqq);
                                        
                                        pt3d.x += (estimatedLocations.at(qqq).x / ((double) estimatedLocations.size()));
                                        pt3d.y += (estimatedLocations.at(qqq).y / ((double) estimatedLocations.size()));
                                        pt3d.z += (estimatedLocations.at(qqq).z / ((double) estimatedLocations.size()));
                                        
                                }

                                tracks.at(indices.at(iii)).set3dLoc(pt3d);
                                
                                //printf("%s << Adding contribution: (%f, %f, %f)\n", __FUNCTION__, pt3d.x, pt3d.y, pt3d.z);
                                
                        }
                        
                        
                        
                        
                }
                
        //      printf("%s << Track (%d) : DONE!\n", __FUNCTION__, indices.at(iii));
        }
        
        //printf("%s << Exiting.\n", __FUNCTION__);
}

unsigned int addNewPoints(vector<featureTrack>& tracks, vector<unsigned int>& indices, cameraParameters& cameraData, Mat *cameras, unsigned int earliest_index, unsigned int latest_index) {

        unsigned int totalTriangulatedPoints = 0;
        
        for (unsigned int iii = 0; iii < indices.size(); iii++) {
                if (tracks.size() < indices.at(iii)) {
                        continue;
                }
                
                if (tracks.at(indices.at(iii)).locations.size() < 2) {
                        continue;
                }
                
                for (unsigned int jjj = 0; jjj < tracks.at(indices.at(iii)).locations.size()-1; jjj++) {
                        
                        //printf("%s << jjj = %d\n", __FUNCTION__, jjj);
                        
                        if (tracks.at(indices.at(iii)).isTriangulated) {
                                continue;
                        }
                        
                        //printf("%s << jjj (continuing)\n", __FUNCTION__, jjj);
                        
                        unsigned int contribCount = 0;
                        Point3d pt3d(0.0, 0.0, 0.0);
                        
                        unsigned int rrr = tracks.at(indices.at(iii)).locations.at(jjj).imageIndex;
                        
                        
                        
                        
                        if (rrr == earliest_index) {
                        
                                if (cameras[rrr].rows != 4) {
                                        printf("%s << Breaking rrr (%d)...\n", __FUNCTION__, rrr);
                                        continue;
                                }
                        
                                //printf("%s << rrr = %d\n", __FUNCTION__, rrr);
                                
                                Mat temp_C0;
                                cameras[rrr].copyTo(temp_C0);
                                
                                for (unsigned int kkk = jjj+1; kkk < tracks.at(indices.at(iii)).locations.size(); kkk++) {
                                        
                                        unsigned int sss = tracks.at(indices.at(iii)).locations.at(kkk).imageIndex;
                                        
                                        
                                        
                                        //printf("%s << sss = %d\n", __FUNCTION__, sss);
                                        
                                        if (sss == latest_index) {
                                        
                                                if (cameras[sss].rows != 4) {
                                                        printf("%s << Breaking sss (%d)...\n", __FUNCTION__, sss);
                                                        continue;
                                                }
                                                
                                                //printf("%s << sss = %d\n", __FUNCTION__, sss);
                                                
                                                Mat temp_C1;
                                                cameras[sss].copyTo(temp_C1);
                                                
                                                // This should be a valid pair of projections to triangulate from for this point..
                                                
                                                Point2f pt1_, pt2_;
                                                Point3d pt3d_;
                                                
                                                pt1_ = tracks.at(indices.at(iii)).getCoord(rrr);
                                                pt2_ = tracks.at(indices.at(iii)).getCoord(sss);
                                                
                                                //printf("%s << Triangulating...\n", __FUNCTION__);
                                                //printf("%s << pts = (%f, %f) & (%f, %f)\n", __FUNCTION__, pt1_.x, pt1_.y, pt2_.x, pt2_.y);
                                                //cout << cameraData.K << endl;
                                                //cout << cameraData.K_inv << endl;
                                                //cout << temp_C1 << endl;
                                                //cout << temp_C0 << endl;
                                                
                                                Triangulate(pt1_, pt2_, cameraData.K, cameraData.K_inv, temp_C1, temp_C0, pt3d_, false);
                                                
                                                //if ((pointIsInFront(temp_C0, pt3d_)) && (pointIsInFront(temp_C1, pt3d_))) {
                                                        pt3d.x += pt3d_.x;
                                                        pt3d.y += pt3d_.y;
                                                        pt3d.z += pt3d_.z;
                                                        contribCount++;
                                                //}
                                        }
                                }
                        }
                        
                        if (contribCount > 0) {
                                
                                //printf("%s << Assigning (%d)\n", __FUNCTION__, indices.at(iii));
                                
                                pt3d.x /= ((double) contribCount); 
                                pt3d.y /= ((double) contribCount);
                                pt3d.z /= ((double) contribCount);
                                
                                tracks.at(indices.at(iii)).set3dLoc(pt3d);
                        }

                        
                        
                }
                
        }
        
        for (unsigned int iii = 0; iii < tracks.size(); iii++) {
                if (tracks.at(iii).isTriangulated) {
                        totalTriangulatedPoints++;
                }
        }
        
        return totalTriangulatedPoints;
        
}

unsigned int putativelyTriangulateNewTracks(vector<featureTrack>& tracks, vector<unsigned int>& indices, cameraParameters& cameraData, Mat *cameras, unsigned int earliest_index, unsigned int latest_index) {
        
        //printf("%s << Entered: (%d, %d, %d, %d)\n", __FUNCTION__, tracks.size(), indices.size(), earliest_index, latest_index);
        
        unsigned int totalTriangulatedPoints = 0;
        
        for (unsigned int iii = 0; iii < indices.size(); iii++) {
                
                //printf("%s << iii = %d\n", __FUNCTION__, iii);
                
                if (tracks.size() < indices.at(iii)) {
                        continue;
                }
                
                if (tracks.at(indices.at(iii)).locations.size() < 2) {
                        continue;
                }
                
                for (unsigned int jjj = 0; jjj < tracks.at(indices.at(iii)).locations.size()-1; jjj++) {
                        
                        //printf("%s << jjj = %d\n", __FUNCTION__, jjj);
                        
                        if (tracks.at(indices.at(iii)).isTriangulated) {
                                continue;
                        }
                        
                        //printf("%s << jjj (continuing)\n", __FUNCTION__, jjj);
                        
                        unsigned int contribCount = 0;
                        Point3d pt3d(0.0, 0.0, 0.0);
                        
                        unsigned int rrr = tracks.at(indices.at(iii)).locations.at(jjj).imageIndex;
                        
                        if (cameras[rrr].rows != 4) {
                                continue;
                        }
                        
                        if ((rrr >= earliest_index) && (rrr <= latest_index)) {
                                
                                //printf("%s << rrr = %d\n", __FUNCTION__, rrr);
                                
                                Mat temp_C0;
                                cameras[rrr].copyTo(temp_C0);
                                
                                for (unsigned int kkk = jjj+1; kkk < tracks.at(indices.at(iii)).locations.size(); kkk++) {
                                        
                                        unsigned int sss = tracks.at(indices.at(iii)).locations.at(kkk).imageIndex;
                                        
                                        if (cameras[sss].rows != 4) {
                                                continue;
                                        }
                                        
                                        //printf("%s << sss = %d\n", __FUNCTION__, sss);
                                        
                                        if ((sss >= earliest_index) && (sss <= latest_index)) {
                                                
                                                //printf("%s << sss = %d\n", __FUNCTION__, sss);
                                                
                                                Mat temp_C1;
                                                cameras[sss].copyTo(temp_C1);
                                                
                                                // This should be a valid pair of projections to triangulate from for this point..
                                                
                                                Point2f pt1_, pt2_;
                                                Point3d pt3d_;
                                                
                                                pt1_ = tracks.at(indices.at(iii)).getCoord(rrr);
                                                pt2_ = tracks.at(indices.at(iii)).getCoord(sss);
                                                
                                                //printf("%s << Triangulating...\n", __FUNCTION__);
                                                //printf("%s << pts = (%f, %f) & (%f, %f)\n", __FUNCTION__, pt1_.x, pt1_.y, pt2_.x, pt2_.y);
                                                //cout << cameraData.K << endl;
                                                //cout << cameraData.K_inv << endl;
                                                //cout << temp_C1 << endl;
                                                //cout << temp_C0 << endl;
                                                
                                                Triangulate(pt1_, pt2_, cameraData.K, cameraData.K_inv, temp_C1, temp_C0, pt3d_, false);
                                                
                                                //if ((pointIsInFront(temp_C0, pt3d_)) && (pointIsInFront(temp_C1, pt3d_))) {
                                                        pt3d.x += pt3d_.x;
                                                        pt3d.y += pt3d_.y;
                                                        pt3d.z += pt3d_.z;
                                                        contribCount++;
                                                //}
                                        }
                                }
                        }
                        
                        if (contribCount > 0) {
                                
                                //printf("%s << Assigning (%d)\n", __FUNCTION__, indices.at(iii));
                                
                                pt3d.x /= ((double) contribCount); 
                                pt3d.y /= ((double) contribCount);
                                pt3d.z /= ((double) contribCount);
                                
                                tracks.at(indices.at(iii)).set3dLoc(pt3d);
                        }

                        
                        
                }
                
        }
        
        for (unsigned int iii = 0; iii < tracks.size(); iii++) {
                if (tracks.at(iii).isTriangulated) {
                        totalTriangulatedPoints++;
                }
        }
        
        return totalTriangulatedPoints;
        
}

void getPointsFromTracks(vector<featureTrack>& tracks, vector<Point2f>& pts1, vector<Point2f>& pts2, int idx1, int idx2) {
        
        // Could make more efficient by only testing tracks that are long enough, but
        // If there are missing frames (acceptable?) then the simple way of achieving this won't work)
        
        //printf("%s << searching for indices (%d) & (%d) [tracks.size() = (%d)]\n", __FUNCTION__, idx1, idx2, tracks.size());
        
        for (unsigned int iii = 0; iii < tracks.size(); iii++) {
                
                //printf("%s << Searching track (%d)...\n", __FUNCTION__, iii);
                
                for (unsigned int kkk = 0; kkk < tracks.at(iii).locations.size(); kkk++) {
                        
                        //printf("%s << Searching element (%d) ; (%d)\n", __FUNCTION__, kkk, tracks.at(iii).locations.at(kkk).imageIndex);
                        
                        //if (tracks.at(iii).locations.at(kkk).imageIndex == 0) {
                                //printf("%s << FOUND A ZERO TRACK!!!\n", __FUNCTION__);
                        //}
                        
                        // If the track extends between the two images of interest
                        if (tracks.at(iii).locations.at(kkk).imageIndex == idx1) {
                                
                                //printf("%s << Found index A (%d)...\n", __FUNCTION__, idx1);
                                
                                for (unsigned int jjj = 0; jjj < tracks.at(iii).locations.size(); jjj++) {
                                        
                                        //printf("%s << Searching element (%d)...\n", __FUNCTION__, jjj);
                                        
                                        if (tracks.at(iii).locations.at(jjj).imageIndex == idx2) {
                                                
                                                //printf("%s << Found match!\n", __FUNCTION__);
                                                
                                                pts1.push_back(tracks.at(iii).locations.at(kkk).featureCoord);
                                                pts2.push_back(tracks.at(iii).locations.at(jjj).featureCoord);
                                                break;
                                        }
                                }
                                
                        }
                }
        }
}

void obtainAppropriateBaseTransformation(Mat& C0, vector<featureTrack>& tracks) {
        
        Point3d centroid, deviations;
        
        int numTriangulatedPts = 0;
        
        for (unsigned int iii = 0; iii < tracks.size(); iii++) {
                if (tracks.at(iii).isTriangulated) {
                        
                        numTriangulatedPts += 1;
                        
                }
        }
        
        if (numTriangulatedPts == 0) {
                C0 = Mat::eye(4, 4, CV_64FC1);
        } else {
                // Find centroid and standard deviations of points
                findCentroidAndSpread(tracks, centroid, deviations);
                
                // Create transformation matrix with a centroid that is distance
                Mat t(4, 1, CV_64FC1), R;
                
                R = Mat::eye(3, 3, CV_64FC1);
                
                t.at<double>(0,0) = centroid.x + 3.0 * deviations.x;
                t.at<double>(0,0) = centroid.y + 3.0 * deviations.y;
                t.at<double>(0,0) = centroid.z + 3.0 * deviations.z;
                t.at<double>(3,0) = 1.0;
                
                compileTransform(C0, R, t);
        }
}

void findCentroidAndSpread(vector<featureTrack>& tracks, Point3d& centroid, Point3d& deviations) {
        
        centroid = Point3d(0.0, 0.0, 0.0);
        deviations = Point3d(0.0, 0.0, 0.0);
        
        double numPoints = 0.00;
        
        for (unsigned int iii = 0; iii < tracks.size(); iii++) {
                if (tracks.at(iii).isTriangulated) {
                        
                        numPoints += 1.00;
                        
                }
        }
        
        if (numPoints == 0.00) {
                return;
        }
        
        for (unsigned int iii = 0; iii < tracks.size(); iii++) {
                
                if (tracks.at(iii).isTriangulated) {
                        
                        Point3d tmpPt;
                        tmpPt = tracks.at(iii).get3dLoc();
                        centroid.x += (tmpPt.x / numPoints);
                        centroid.y += (tmpPt.y / numPoints);
                        centroid.z += (tmpPt.z / numPoints);
                        
                }
                
        }
        
        for (unsigned int iii = 0; iii < tracks.size(); iii++) {
                
                if (tracks.at(iii).isTriangulated) {
                        Point3d tmpPt;
                        tmpPt = tracks.at(iii).get3dLoc();
                        deviations.x += (pow((tmpPt.x - centroid.x), 2.0) / numPoints);
                        deviations.y += (pow((tmpPt.y - centroid.y), 2.0) / numPoints);
                        deviations.z += (pow((tmpPt.z - centroid.z), 2.0) / numPoints);
                }
                
        }
        
        deviations.x = pow(deviations.x, 0.5);
        deviations.y = pow(deviations.y, 0.5);
        deviations.z = pow(deviations.z, 0.5);
}

void getPoints3dFromTracks(vector<featureTrack>& tracks, vector<Point3d>& cloud, int idx1, int idx2) {
        
        cloud.clear();
        
        for (unsigned int iii = 0; iii < tracks.size(); iii++) {
                
                for (unsigned int kkk = 0; kkk < tracks.at(iii).locations.size()-1; kkk++) {
                        // If the track extends between the two images of interest
                        if (tracks.at(iii).locations.at(kkk).imageIndex == idx1) {
                                
                                for (unsigned int jjj = kkk+1; jjj < tracks.at(iii).locations.size(); jjj++) {
                                        
                                        if (tracks.at(iii).locations.at(jjj).imageIndex == idx2) {
                                                cloud.push_back(tracks.at(iii).get3dLoc());
                                                break;
                                        }
                                }
                                
                        }
                }
                
                

        }
        
}


void findFourTransformations(Mat *C, const Mat& E, const Mat& K, const vector<Point2f>& pts1, const vector<Point2f>& pts2) {
        
        
        Mat I, negI, t33, W, Winv, Z, Kinv, zeroTrans, R, t, Rvec;
        
        Kinv = K.inv();
        zeroTrans = Mat::zeros(3, 1, CV_64FC1);
        I = Mat::eye(3, 3, CV_64FC1);
        negI = -I;
        
        getWandZ(W, Winv, Z);
        
        //cout << __FUNCTION__ << " << W = " << W << endl;
        //cout << __FUNCTION__ << " << Winv = " << Winv << endl;
        
        //R = svd.u * W * svd.vt;
        //t = svd.u.col(2);
        
        //cout << __FUNCTION__ << " << R = " << R << endl;
        
        //Rodrigues(R, Rvec);
        
        //cout << __FUNCTION__ << " << Rvec = " << Rvec << endl;
        
        //cout << __FUNCTION__ << " << t = " << t << endl;
        
        Mat c;
        //compileTransform(c, R, t);
        
        //cout << __FUNCTION__ << " << c = " << c << endl;
        
        Mat u_transposePos, u_transposeNeg;
        SVD svdPos, svdNeg;
        
        svdPos = SVD(E);
        svdNeg = SVD(-E);
                
        transpose(svdPos.u, u_transposePos);
        transpose(svdNeg.u, u_transposeNeg);
        
        double detVal;
        
        for (unsigned int zzz = 0; zzz < 4; zzz++) {
                
                //printf("%s << Loop [%d]\n", __FUNCTION__, zzz);
                
                switch (zzz) {
                        case 0:
                                //t33 = v * W * SIGMA * vt;
                                t33 = svdPos.u * Z * u_transposePos;
                                R = svdPos.u * Winv * svdPos.vt;
                                detVal = determinant(R);
                                if (detVal < 0) {
                                        t33 = svdNeg.u * Z * u_transposeNeg;
                                        R = svdNeg.u * Winv * svdNeg.vt;
                                }
                                break;
                        case 1:
                                //t33 = v * W_inv * SIGMA * vt;
                                t33 = negI * svdPos.u * Z * u_transposePos;
                                R = svdPos.u * Winv * svdPos.vt;
                                detVal = determinant(R);
                                if (detVal < 0) {
                                        t33 = negI * svdNeg.u * Z * u_transposeNeg;
                                        R = svdNeg.u * Winv * svdNeg.vt;
                                }
                                break;
                        case 2:
                                //t33 = v * W * SIGMA * vt;
                                t33 = svdPos.u * Z * u_transposePos;
                                R = svdPos.u * W * svdPos.vt;
                                detVal = determinant(R);
                                if (detVal < 0) {
                                        t33 = svdNeg.u * Z * u_transposeNeg;
                                        R = svdNeg.u * W * svdNeg.vt;
                                }
                                break;
                        case 3:
                                //t33 = v * W_inv * SIGMA * vt;
                                t33 = negI * svdPos.u * Z * u_transposePos;
                                R = svdPos.u * W * svdPos.vt;
                                detVal = determinant(R);
                                if (detVal < 0) {
                                        t33 = negI * svdNeg.u * Z * u_transposeNeg;
                                        R = svdNeg.u * W * svdNeg.vt;
                                }
                                break;
                        default:
                                break;
                }
                
                // Converting 3x3 t mat to vector
                t = Mat::zeros(3, 1, CV_64F);
                t.at<double>(0,0) = -t33.at<double>(1,2); // originally had -ve
                t.at<double>(1,0) = t33.at<double>(0,2); // originall had a +ve
                t.at<double>(2,0) = -t33.at<double>(0,1); // originally had -ve
                
                //cout << __FUNCTION__ << " << t[" << zzz << "] = " << t << endl;
                
                if (abs(1.0 - detVal) > 0.01) {
                        //printf("%s << Solution [%d] is invalid - det(R) = %f\n", __FUNCTION__, zzz, detVal);
                } else {
                        //printf("%s << Solution [%d] det(R) = %f\n", __FUNCTION__, zzz, detVal);
                }
                
                //compileTransform(c, R, -R*t);
                compileTransform(c, R, t);
                
                //char printSummary[256];
                //summarizeTransformation(c, printSummary);
                //printf("%s << Original c[%d] = %s\n", __FUNCTION__, zzz, printSummary);
                
                //c = c.inv();
                
                c.copyTo(C[zzz]);
                
                //cout << __FUNCTION__ << " << Czzz = " << C[zzz] << endl;
                
        }

}

void projectionToTransformation(const Mat& proj, Mat& trans) {
        trans = Mat::eye(4, 4, CV_64FC1);
        
        trans.at<double>(0,0) = proj.at<double>(0,0);
        trans.at<double>(0,1) = proj.at<double>(0,1);
        trans.at<double>(0,2) = proj.at<double>(0,2);
        trans.at<double>(0,3) = proj.at<double>(0,3);
        
        trans.at<double>(1,0) = proj.at<double>(1,0);
        trans.at<double>(1,1) = proj.at<double>(1,1);
        trans.at<double>(1,2) = proj.at<double>(1,2);
        trans.at<double>(1,3) = proj.at<double>(1,3);
        
        trans.at<double>(2,0) = proj.at<double>(2,0);
        trans.at<double>(2,1) = proj.at<double>(2,1);
        trans.at<double>(2,2) = proj.at<double>(2,2);
        trans.at<double>(2,3) = proj.at<double>(2,3);
}

void transformationToProjection(const Mat& trans, Mat& proj) {
        proj = Mat::zeros(3, 4, CV_64FC1);
        
        proj.at<double>(0,0) = trans.at<double>(0,0);
        proj.at<double>(0,1) = trans.at<double>(0,1);
        proj.at<double>(0,2) = trans.at<double>(0,2);
        proj.at<double>(0,3) = trans.at<double>(0,3);
        
        proj.at<double>(1,0) = trans.at<double>(1,0);
        proj.at<double>(1,1) = trans.at<double>(1,1);
        proj.at<double>(1,2) = trans.at<double>(1,2);
        proj.at<double>(1,3) = trans.at<double>(1,3);
        
        proj.at<double>(2,0) = trans.at<double>(2,0);
        proj.at<double>(2,1) = trans.at<double>(2,1);
        proj.at<double>(2,2) = trans.at<double>(2,2);
        proj.at<double>(2,3) = trans.at<double>(2,3);
                
}

bool pointIsInFront(const Mat& C, const Point3d& pt) {
        
        Point3d p1;
        
        transfer3dPoint(pt, p1, C);

        if (p1.z > 0) {
                return true;
        } else {
                
                return false;
                
        }
        
        
}

// This function is when you have the 3D points and both transformation matrices
int pointsInFront(const Mat& C1, const Mat& C2, const vector<Point3d>& pts) {
        int retVal = 0;
        
        vector<Point3d> pts1, pts2;
        
        Mat R1, t1, R2, t2;
        decomposeTransform(C1, R1, t1);
        decomposeTransform(C2, R2, t2);
        
        double rot1, rot2;
        rot1 = getRotationInDegrees(R1);
        rot2 = getRotationInDegrees(R2);
        
        printf("%s << Two rotations = (%f, %f)\n", __FUNCTION__, rot1, rot2);
        
        // maybe something to do with intrinsics matrix....
        
        // want to transfer the 3D point locations into the co-ordinate system of each camera
        transfer3DPoints(pts, pts1, C1);
        transfer3DPoints(pts, pts2, C2);
        
        for (unsigned int iii = 0; iii < pts.size(); iii++) {
                
                //printf("%s << [%d](%f, %f, %f)-(%f, %f, %f)\n", __FUNCTION__, iii, pts1.at(iii).x, pts1.at(iii).y, pts1.at(iii).z, pts2.at(iii).x, pts2.at(iii).y, pts2.at(iii).z);
                
                if ((pts1.at(iii).z > 0) && (pts2.at(iii).z > 0)) {
                        
                        if (retVal < 5) {
                                printf("%s << Point location is (%f, %f, %f)\n", __FUNCTION__, pts.at(iii).x, pts.at(iii).y, pts.at(iii).z);
                                printf("%s << Rel #1 is (%f, %f, %f)\n", __FUNCTION__, pts1.at(iii).x, pts1.at(iii).y, pts1.at(iii).z);
                                printf("%s << Rel #2 is (%f, %f, %f)\n", __FUNCTION__, pts2.at(iii).x, pts2.at(iii).y, pts2.at(iii).z);
                        }
                        
                        retVal++;
                }
        }
        
        return retVal;
}

// This function is when you have the relative transformation matrix, cam matrix, and 2D locations of points..
int pointsInFront(const Mat& C1, const Mat& K, const vector<Point2f>& pts1, const vector<Point2f>& pts2) {
        
        int retVal = 0;
        
        Mat Kinv;
        Kinv = K.inv();
        
        // Representing initial camera relative to itself
        Mat P0, C0;
        initializeP0(P0);
        projectionToTransformation(P0, C0);
        
        // Representing second camera relative to first camera
        Mat P1;
        transformationToProjection(C1, P1);
        

        vector<Point3d> pc1, pc2;
        vector<Point2f> correspImg1Pt, correspImg2Pt;   

        // Get 3D locations of points relative to first camera
        TriangulatePoints(pts1, pts2, K, Kinv, P0, P1, pc1, correspImg1Pt);
        
        // Transfer 3D locations so that they are relative to second camera
        Mat C1inv;
        C1inv = C1.inv();
        
        // src, dst, transform : if C1 represents transform from world to cam 1...
        transfer3DPoints(pc1, pc2, C1); // trying C1inv...
        
        //Mat R1, t1;
        //decomposeTransform(C, R1, t1);
        //double deg1 = getRotationInDegrees(R1);
        //printf("%s << Forwards rotation = %f\n", __FUNCTION__, deg1);

        
        //printf("%s << DEBUG %d\n", __FUNCTION__, 2);
        //Cinv = C.inv();
        //transformationToProjection(Cinv, P1b);
        
        //Mat R2, t2;
        //decomposeTransform(Cinv, R2, t2);
        //double deg2 = getRotationInDegrees(R2);
        //printf("%s << Backwards rotation = %f\n", __FUNCTION__, deg2);
        
        //printf("%s << DEBUG %d\n", __FUNCTION__, 3);
        //TriangulatePoints(pts2, pts1, K, Kinv, P0, P1b, pc2, correspImg2Pt);

        //printf("%s << DEBUG %d\n", __FUNCTION__, 4);
        
        int inFront1 = 0, inFront2 = 0;
        
        for (unsigned int iii = 0; iii < pc1.size(); iii++) {
                
                // Check z-coordinate in both clouds to make sure it's > 0
                
                //printf("%s << pc1 = (%f, %f, %f); pc2 = (%f, %f, %f)\n", __FUNCTION__, pc1.at(iii).x, pc1.at(iii).y, pc1.at(iii).z, pc2.at(iii).x, pc2.at(iii).y, pc2.at(iii).z);
                
                if (pc1.at(iii).z > 0) {
                        inFront1++;
                }
                
                if (pc2.at(iii).z > 0) {
                        inFront2++;
                }
                
                if ((pc1.at(iii).z > 0) && (pc2.at(iii).z > 0)) {
                        retVal++;
                }
                
        }
        
        //printf("%s << Pts in front of cams: %d, %d\n", __FUNCTION__, inFront1, inFront2);
        
        return retVal;
        
}

void convertPoint3dToMat(const Point3d& src, Mat& dst) {
        dst = Mat::zeros(4, 1, CV_64FC1);
        
        dst.at<double>(3, 0) = 1.0;

        dst.at<double>(0,0) = src.x;
        dst.at<double>(1,0) = src.y;
        dst.at<double>(2,0) = src.z;
}

void transfer3dPoint(const Point3d& src, Point3d& dst, const Mat& C) {
        
        Mat pt1, pt2;
        
        pt1 = Mat::zeros(4, 1, CV_64FC1);
        pt2 = Mat::zeros(4, 1, CV_64FC1);
        
        pt1.at<double>(3, 0) = 1.0;
        pt2.at<double>(3, 0) = 1.0;

        
        pt1.at<double>(0,0) = src.x;
        pt1.at<double>(1,0) = src.y;
        pt1.at<double>(2,0) = src.z;
        
        pt2 = C * pt1;
        
        dst = Point3d(pt2.at<double>(0,0), pt2.at<double>(1,0), pt2.at<double>(2,0));
        
}

void transfer3DPoints(const vector<Point3d>& src, vector<Point3d>& dst, const Mat& C) {
        
        Mat pt1, pt2;
        
        pt1 = Mat::zeros(4, 1, CV_64FC1);
        pt2 = Mat::zeros(4, 1, CV_64FC1);
        
        pt1.at<double>(3, 0) = 1.0;
        pt2.at<double>(3, 0) = 1.0;
        
        Point3d shiftedPoint;
                
        for (unsigned int iii = 0; iii < src.size(); iii++) {
                pt1.at<double>(0,0) = src.at(iii).x;
                pt1.at<double>(1,0) = src.at(iii).y;
                pt1.at<double>(2,0) = src.at(iii).z;
                
                pt2 = C * pt1;          // pt2 = C.inv() * pt1;
                
                //printf("%s << pt2.at<double>(3,0) = %f\n", __FUNCTION__, pt2.at<double>(3,0));
                
                shiftedPoint.x = pt2.at<double>(0,0);
                shiftedPoint.y = pt2.at<double>(1,0);
                shiftedPoint.z = pt2.at<double>(2,0);
                
                dst.push_back(shiftedPoint);
        }
        
        
        
}

void quaternionToMatrix(const Quaterniond& quat, Mat& mat) {
        
        Matrix3d m = quat.toRotationMatrix();
        
        mat = Mat::zeros(3, 3, CV_64FC1);
        
        mat.at<double>(0,0) = m(0,0);
        mat.at<double>(0,1) = m(0,1);
        mat.at<double>(0,2) = m(0,2);
        
        mat.at<double>(1,0) = m(1,0);
        mat.at<double>(1,1) = m(1,1);
        mat.at<double>(1,2) = m(1,2);
        
        mat.at<double>(2,0) = m(2,0);
        mat.at<double>(2,1) = m(2,1);
        mat.at<double>(2,2) = m(2,2);
        
        return;
        
        Quaterniond quat_N(quat);
        
        quat_N.normalize();
        
        double x = quat_N.x();
        double y = quat_N.y();
        double z = quat_N.z();
        double w = quat_N.w();
        
        mat.at<double>(0,0) =  1.0 - 2.0*y*y - 2.0*z*z;
        mat.at<double>(0,1) =  2.0*x*y - 2.0*z*w;
        mat.at<double>(0,2) =  2.0*x*z + 2.0*y*w;
        
        mat.at<double>(1,0) =  2.0*x*y + 2.0*z*w;
        mat.at<double>(1,1) =  1.0 - 2.0*x*x - 2.0*z*z;
        mat.at<double>(1,2) =  2.0*y*z - 2.0*x*w;
        
        mat.at<double>(2,0) =  2.0*x*z - 2.0*y*w;
        mat.at<double>(2,1) =  2.0*y*z + 2.0*x*w;
        mat.at<double>(2,2) =  1.0 - 2.0*x*x - 2.0*y*y; 

}

void decomposeTransform(const Mat& c, Mat& R, Mat& t) {
        R = Mat::zeros(3, 3, CV_64FC1);
        t = Mat::zeros(3, 1, CV_64FC1);
        
        R.at<double>(0,0) = c.at<double>(0,0);
        R.at<double>(0,1) = c.at<double>(0,1);
        R.at<double>(0,2) = c.at<double>(0,2);
        
        R.at<double>(1,0) = c.at<double>(1,0);
        R.at<double>(1,1) = c.at<double>(1,1);
        R.at<double>(1,2) = c.at<double>(1,2);
        
        R.at<double>(2,0) = c.at<double>(2,0); 
        R.at<double>(2,1) = c.at<double>(2,1); 
        R.at<double>(2,2) = c.at<double>(2,2);
        
        t.at<double>(0,0) = c.at<double>(0,3);
        t.at<double>(1,0) = c.at<double>(1,3);
        t.at<double>(2,0) = c.at<double>(2,3);
        
}
void compileTransform(Mat& c, const Mat& R, const Mat& t) {
        c = Mat::zeros(4, 4, CV_64FC1);
        
        c.at<double>(0,0) = R.at<double>(0,0);
        c.at<double>(0,1) = R.at<double>(0,1); 
        c.at<double>(0,2) = R.at<double>(0,2); 
        
        c.at<double>(1,0) = R.at<double>(1,0); 
        c.at<double>(1,1) = R.at<double>(1,1); 
        c.at<double>(1,2) = R.at<double>(1,2); 
        
        c.at<double>(2,0) = R.at<double>(2,0); 
        c.at<double>(2,1) = R.at<double>(2,1); 
        c.at<double>(2,2) = R.at<double>(2,2);
        
        c.at<double>(0,3) = t.at<double>(0,0);
        c.at<double>(1,3) = t.at<double>(1,0);
        c.at<double>(2,3) = t.at<double>(2,0);
        
        c.at<double>(3,3) = 1.0;
}

double calcInlierGeometryDistance(vector<Point2f>& points1, vector<Point2f>& points2, Mat& mat, Mat& mask, int distMethod) {
        
        double error = 0.0;
        
        int inlierCounter = 0;

        for (unsigned int iii = 0; iii < points1.size(); iii++) {
                if (mask.at<char>(iii, 0) > 0) {
                        
                        error += calcGeometryDistance(points1.at(iii), points2.at(iii), mat, distMethod);
                        
                        inlierCounter++;
                }
        }
        
        error /= (double) inlierCounter;

        return error;
}

double calcGeometryScore(vector<Point2f>& points1, vector<Point2f>& points2, Mat& F, Mat& Fmask, Mat& H, Mat& Hmask) {
        double gScore = 0.0;
        
        int inliers_H = 0, inliers_F = 0;
        
        /*
        for (int iii = 0; iii < points1.size(); iii++) {
                if (Hmask.at<char>(iii, 0) > 0) {
                        inliers_H++;
                }
                
                if (Fmask.at<char>(iii, 0) > 0) {
                        inliers_F++;
                }               
        }
        * */
        
        inliers_H = countNonZero(Hmask);
        inliers_F = countNonZero(Fmask);
        
        printf("%s << Inliers: %d (H), %d (F)\n", __FUNCTION__, inliers_H, inliers_F);
        
        double sampson_H, sampson_F;
        
        sampson_H = calcSampsonError(points1, points2, H, Hmask);
        sampson_F = calcSampsonError(points1, points2, F, Fmask);
        
        double sampson_H2 = 0.0, sampson_F2 = 0.0;
        
        printf("%s << Sampson errors: %f (H), %f (F)\n", __FUNCTION__, sampson_H, sampson_F);
        
        for (unsigned int iii = 0; iii < points1.size(); iii++) {
                sampson_H2 += calcSampsonDistance(points1.at(iii), points2.at(iii), H) / (double)points1.size();
                sampson_F2 += calcSampsonDistance(points1.at(iii), points2.at(iii), F) / (double)points1.size();
        }
        
        printf("%s << Sampson errors2: %f (H), %f (F)\n", __FUNCTION__, sampson_H2, sampson_F2);
        
        
        gScore = (((double) inliers_F) / ((double) inliers_H)) * ( sampson_H / pow(sampson_F, 0.5));
        
        printf("%s << gScore = %f\n", __FUNCTION__, gScore);
        
        return gScore;
}

double calcFrameScore(double geomScore, int numFeatures, int numTracks) {
        
        double frameScore;
        
        frameScore = geomScore * ((double) numTracks) / ((double) numFeatures);
        
        return frameScore;
}
 
double calcSampsonError(vector<Point2f>& points1, vector<Point2f>& points2, Mat& H, Mat& Hmask) {
        double sampsonError = 0.0;
        
        int inlierCounter = 0;

        
        for (unsigned int iii = 0; iii < points1.size(); iii++) {
                if (Hmask.at<char>(iii, 0) > 0) {
                        
                        sampsonError += lourakisSampsonError(points1.at(iii), points2.at(iii), H);
                        
                        inlierCounter++;
                }
        }
        
        sampsonError /= (double) inlierCounter;

        return sampsonError;

}

// http://www.mathworks.com.au/help/toolbox/vision/ref/estimatefundamentalmatrix.html
double calcSampsonDistance(Point2f& pt1, Point2f& pt2, Mat& F) {
        
        double dist;
        
        Mat point1(1, 3, CV_64FC1), point2(1, 3, CV_64FC1);
        
        point1.at<double>(0, 0) = (double) pt1.x;
        point1.at<double>(0, 1) = (double) pt1.y;
        point1.at<double>(0, 2) = 1.0;
        
        Mat t1;
        transpose(point1, t1);
        
        point2.at<double>(0, 0) = (double) pt2.x;
        point2.at<double>(0, 1) = (double) pt2.y;
        point2.at<double>(0, 2) = 1.0;
        
        Mat a, b, c;
        
        a = point2 * F * t1;
        b = F * t1;
        c = point2 * F;
        
        double val1 = pow(a.at<double>(0, 0), 2.0);
        double val2 = 1.0 / ((pow(b.at<double>(0, 0), 2.0)) + (pow(b.at<double>(1, 0), 2.0)));
        double val3 = 1.0 / ((pow(c.at<double>(0, 0), 2.0)) + (pow(c.at<double>(1, 0), 2.0)));
        
        dist = val1 * (val2 + val3);
        
        return dist;
        
}

double calcGeometryScore(int numInliers_H, int numInliers_F, double sampsonError_H, double sampsonError_F) {
        double geometryScore = 0.0;
        
        geometryScore = ((double) numInliers_F / (double) numInliers_H) * sampsonError_H / pow(sampsonError_F, 0.5);
        
        return geometryScore;
}

void assignIntrinsicsToP0(Mat& P0, const Mat& K) {
        P0 = Mat::zeros(3, 4, CV_64FC1);
        
        for (int iii = 0; iii < 3; iii++) {
                for (int jjj = 0; jjj < 3; jjj++) {
                        P0.at<double>(iii,jjj) = K.at<double>(iii,jjj);
                }
        }
        
}

double getQuaternionAngle(const Quaterniond& q1, const Quaterniond& q2) {
        double angle, dot_prod;
        
        dot_prod = dotProduct(q1, q2);
        
        angle = 2.0 * acos(abs(dot_prod));
        
        return angle;
}

double dotProduct(const Quaterniond& q1, const Quaterniond& q2) {
        double prod;
        
        prod = q1.x()*q2.x() + q1.y()*q2.y() + q1.z()*q2.z() + q1.w()*q2.w();
        
        return prod;
}

void combineTransforms(Mat& CN, const Mat& C0, const Mat& C1) {
        CN = Mat::eye(4, 4, CV_64FC1);
        
        CN = C0 * C1;
}

double dotProduct(const Mat& vec1, const Mat& vec2) {
        double prod;
        
        prod = vec1.at<double>(0,0)*vec2.at<double>(0,0) + vec1.at<double>(0,0)*vec2.at<double>(0,0) + vec1.at<double>(0,0)*vec2.at<double>(0,0);

        return prod;

}

double getRotationInDegrees(const Mat& R) {
        Quaterniond rotation;
        matrixToQuaternion(R, rotation);
        Quaterniond dq = defaultQuaternion();
        double qa = getQuaternionAngle(rotation, dq) * 180.0 / M_PI;
        
        return qa;

}

double getDistanceInUnits(const Mat& t) {
        double retVal = 0.0;
        
        retVal += pow(t.at<double>(0,0), 2.0);
        retVal += pow(t.at<double>(1,0), 2.0);
        retVal += pow(t.at<double>(2,0), 2.0);
        retVal = pow(retVal, 0.5);
        
        return retVal;
}


void addFixedCamera(SysSBA& sys, cameraParameters& cameraData, const Mat& C) {
        addBlankCamera(sys, cameraData, true);
        updateCameraNode_2(sys, C, sys.nodes.size()-1);
}

void addNewCamera(SysSBA& sys, cameraParameters& cameraData, const Mat& C) {
        addBlankCamera(sys, cameraData, false);
        updateCameraNode_2(sys, C, sys.nodes.size()-1);
}

void convertVec4dToMat(const Vector4d& vec4, Mat& mat) {
        mat = Mat::zeros(3, 1, CV_64FC1);
        
        mat.at<double>(0,0) = vec4.x();
        mat.at<double>(1,0) = vec4.y();
        mat.at<double>(2,0) = vec4.z();
        
}

void updateTracks(vector<featureTrack>& trackVector, const SysSBA& sys) {
        Point3d tmpPt;
        for (unsigned int iii = 0; iii < sys.tracks.size(); iii++) {
                if (iii < trackVector.size()) {
                        tmpPt.x = sys.tracks[iii].point.x();
                        tmpPt.y = sys.tracks[iii].point.y();
                        tmpPt.z = sys.tracks[iii].point.z();
                        trackVector.at(iii).set3dLoc(tmpPt);
                } // otherwise these points mightn't belong in the track vector..
                
        }
}

void retrieveAllPoints(vector<Point3d>& pts, const SysSBA& sys) {
        
        Point3d point_3;
        
        for (unsigned int iii = 0; iii < sys.tracks.size(); iii++) {
                point_3.x = sys.tracks[iii].point.x();
                point_3.y = sys.tracks[iii].point.y();
                point_3.z = sys.tracks[iii].point.z();
                        
                pts.push_back(point_3);
        }
}

void retrieveCameraPose(SysSBA& sys, Mat& camPose, int idx) {
        
        Mat R, t, C;
        quaternionToMatrix(sys.nodes[idx].qrot, R);
        convertVec4dToMat(sys.nodes[idx].trans, t);
        
        compileTransform(C, R, t);
        C.copyTo(camPose);
        
}

void retrieveCamera(Mat& camera, const SysSBA& sys, unsigned int idx) {

        Mat R, t, C;
        quaternionToMatrix(sys.nodes[idx].qrot, R);
        convertVec4dToMat(sys.nodes[idx].trans, t);
        compileTransform(C, R, t);
        C.copyTo(camera);
        
}

void retrieveAllCameras(Mat *allCameraPoses, const SysSBA& sys) {
        
        for (unsigned int iii = 0; iii < sys.nodes.size(); iii++) {
                //if (allCameraPoses[iii].rows < 4) {
                        
                retrieveCamera(allCameraPoses[iii], sys, iii);
                        
                /*
                Mat R, t, C;
                quaternionToMatrix(sys.nodes[iii].qrot, R);
                convertVec4dToMat(sys.nodes[iii].trans, t);
                
                compileTransform(C, R, t);
                C.copyTo(allCameraPoses[iii]);
                */
                //} 
        }
}

void assignTracksToSBA(SysSBA& sys, vector<featureTrack>& trackVector, int maxIndex) {
        
        sys.tracks.clear();
        
        // ConnMat ??
        sys.connMat.clear();
        
        
        //printf("%s << Assigning (%d) tracks...\n", __FUNCTION__, trackVector.size());
        
        
        // For each track
        for (unsigned int iii = 0; iii < trackVector.size(); iii++) {
                
                // The first occurence of this feature has to be before the "maxIndex"
                if (trackVector.at(iii).locations.at(0).imageIndex < maxIndex) {
                        Point3d tmp3dPt = trackVector.at(iii).get3dLoc();
                        Vector4d temppoint(tmp3dPt.x, tmp3dPt.y, tmp3dPt.z, 1.0);
                        sys.addPoint(temppoint);
                }
                
        }
        
        // For each track
        for (unsigned int iii = 0; iii < trackVector.size(); iii++) {
                Vector2d proj;
                
                // The first occurence of this feature has to be before the "maxIndex"
                if (trackVector.at(iii).locations.at(0).imageIndex < maxIndex) {
                        
                        // For each projection
                        for (unsigned int jjj = 0; jjj < trackVector.at(iii).locations.size(); jjj++) {

                                // Projection cannot be in an image beyond the currently "active" camera
                                if (trackVector.at(iii).locations.at(jjj).imageIndex < (maxIndex+1)) {
                                        proj.x() = trackVector.at(iii).locations.at(jjj).featureCoord.x;
                                        proj.y() = trackVector.at(iii).locations.at(jjj).featureCoord.y;
                                        
                                        sys.addMonoProj(trackVector.at(iii).locations.at(jjj).imageIndex, iii, proj);
                                }

                                
                        }
                }
                
                                                        
        }
        
}

void estimateNewPose(vector<featureTrack>& tracks, Mat& K, int idx, Mat& pose) {
        // Find any tracks that have at least 3 "sightings" (so that 3d location is known)
        // Use 3D points and 2d locations to estimate R and t and then get pose
        
        vector<Point3d> worldPoints;
        vector<Point2f> imagePoints1, imagePoints2;
        
        Mat blankCoeffs = Mat::zeros(1, 8, CV_64FC1);
        
        for (unsigned int iii = 0; iii < tracks.size(); iii++) {
                if (tracks.at(iii).locations.size() >= 2) {
                        
                        int finalIndex = tracks.at(iii).locations.size()-1;
                        
                        if (tracks.at(iii).locations.at(finalIndex).imageIndex == idx) {
                                //Point3d tmpPt(tracks.at(iii).xyzEstimate.x, tracks.at(iii).xyzEstimate.y, tracks.at(iii).xyzEstimate.z);
                                worldPoints.push_back(tracks.at(iii).get3dLoc());
                                Point2f tmpPt2(tracks.at(iii).locations.at(finalIndex-1).featureCoord.x, tracks.at(iii).locations.at(finalIndex-1).featureCoord.y);
                                imagePoints1.push_back(tmpPt2);
                                tmpPt2 = Point2f(tracks.at(iii).locations.at(finalIndex).featureCoord.x, tracks.at(iii).locations.at(finalIndex).featureCoord.y);
                                imagePoints2.push_back(tmpPt2);
                        }
                        
                }
        }
        
        
        Mat Rvec, R, t;

        vector<Point3f> objectPoints;
        Point3f tmpPt;
        
        for (unsigned int jjj = 0; jjj < worldPoints.size(); jjj++) {
                tmpPt = Point3f((float) worldPoints.at(jjj).x, (float) worldPoints.at(jjj).y, (float) worldPoints.at(jjj).z);
                objectPoints.push_back(tmpPt);
        }
        
        //solvePnPRansac(InputArray objectPoints, InputArray imagePoints, InputArray cameraMatrix, InputArray distCoeffs, OutputArray rvec, OutputArray tvec, bool useExtrinsicGuess=false, int iterationsCount=100, float reprojectionError=8.0, int minInliersCount=100, OutputArray inliers=noArray() );
        solvePnPRansac(objectPoints, imagePoints2, K, blankCoeffs, Rvec, t);
        Rodrigues(Rvec, R);

        compileTransform(pose, R, t);
        
}

void addNewPoints(SysSBA& sys, const vector<Point3d>& pc) {
        
        Vector2d proj;
        
        proj.x() = 0.0;
        proj.y() = 0.0;
        
        for (unsigned int iii = 0; iii < pc.size(); iii++) {
                Vector4d temppoint(pc.at(iii).x, pc.at(iii).y, pc.at(iii).z, 1.0);
                        
                sys.addPoint(temppoint);
                
                sys.addMonoProj(0, iii, proj);
        }
}

int addToTracks(SysSBA& sys, int im1, vector<Point2f>& pts1, int im2, vector<Point2f>& pts2) {
        
        //printSystemSummary(sys);
        
        //printf("%s << Entered.\n", __FUNCTION__);
        
        Vector2d proj;
        
        //printf("%s << oldTrackCount = %d\n", __FUNCTION__, oldTrackCount);
        //printf("%s << nodes = %d\n", __FUNCTION__, sys.nodes.size());
        
        for (unsigned int iii = 0; iii < pts1.size(); iii++) {
                
                bool pointInTracks = false;
                
                //printf("%s << [%d][%d][%d] : tracks = %d\n", __FUNCTION__, im1, im2, iii, sys.tracks.size());
                
                for (unsigned int ttt = 0; ttt < sys.tracks.size(); ttt++) { // NOT oldTrackCount, in case of automatic re-ordering...
                        
                        //printf("%s << old_track(%d) size = %d\n", __FUNCTION__, ttt, sys.tracks.at(ttt).projections.size());
                        
                        int finalIndex = sys.tracks.at(ttt).projections.size() - 1;
                        
                        //printf("%s << [%d] finalIndex = %d\n", __FUNCTION__, ttt, finalIndex);
                        
                        if (finalIndex > -1) {
                                
                                //printf("%s << DEBUG[%d] (%d vs %d)\n", __FUNCTION__, 0, finalIndex, sys.tracks.at(ttt).projections.size());
                                
                                printf("%s << Attempting [%d][%d][%d][%d]\n", __FUNCTION__, im1, im2, iii, ttt);
                                printf("%s << ndi(%d / %d) = \n", __FUNCTION__, finalIndex, sys.tracks.at(ttt).projections.size());
                                
                                printf("%s << sizeof() %d\n", __FUNCTION__, sizeof(sys.tracks.at(ttt).projections));
                                
                                if (finalIndex > 0) {
                                        printf("%s << (-1) %d\n", __FUNCTION__, sys.tracks.at(ttt).projections.at(finalIndex-1).ndi);
                                }
                                
                                printf("%s << %d\n", __FUNCTION__, sys.tracks.at(ttt).projections.at(finalIndex).ndi);
                                
                                if ((im1 == 2) && (im2 == 3) && (iii == 16) && (ttt == 68)) {
                                        //cin.get();
                                }
                                // 3, 4, 0, #49/50 track
                                
                                // why would it say that projections is large enough, but then you can't read ndi...?
                                // or is size returning the wrong info?
                                
                                if (sys.tracks.at(ttt).projections.at(finalIndex).ndi == im1) {

                                        //printf("%s << DEBUG[%d]\n", __FUNCTION__, 1);
                                        
                                        if ((sys.tracks.at(ttt).projections.at(finalIndex).kp.x() == pts1.at(iii).x) && (sys.tracks.at(ttt).projections.at(finalIndex).kp.y() == pts1.at(iii).y)) {
                                                
                                                //printf("%s << DEBUG[%d]\n", __FUNCTION__, 2);
                                                
                                                //printf("%s << Point exists in this track (%d)!\n", __FUNCTION__, ttt);
                                                
                                                pointInTracks = true;
                                                
                                                proj.x() = pts2.at(iii).x;
                                                proj.y() = pts2.at(iii).y;
                                                
                                                if (!sys.addMonoProj(im2, ttt, proj)) {
                                                        printf("%s << Adding point failed!\n", __FUNCTION__);
                                                }
                                                
                                                //printf("%s << Projection added.\n", __FUNCTION__);
                                                
                                                break;
                                                
                                        }
                                        
                                        //printf("%s << DEBUG[%d]\n", __FUNCTION__, 3);
                                }
                                
                                //printf("%s << DEBUG[%d]\n", __FUNCTION__, 4);
                        }
                        
                        

                }
                
                if (!pointInTracks) {
                        
                        //printf("%s << Point not in any of the tracks...\n", __FUNCTION__);
                        
                        int newTrackNum = sys.tracks.size();
                        
                        Vector4d temppoint(0.0, 0.0, 0.0, 1.0);
                        
                        //temppoint.x() = 0.0;
                        //temppoint.y() = 0.0;
                        //temppoint.z() = 0.0;
                        sys.addPoint(temppoint);
                        
                        // Then add a track?
                        //sys.tracks.push_back(Track(temppoint));
                        
                        proj.x() = pts1.at(iii).x;
                        proj.y() = pts1.at(iii).y;
                        
                        if (!sys.addMonoProj(im1, newTrackNum, proj)) {
                                printf("%s << Adding point failed!\n", __FUNCTION__);
                        }
                        
                        proj.x() = pts2.at(iii).x;
                        proj.y() = pts2.at(iii).y;
                        
                        if (!sys.addMonoProj(im2, newTrackNum, proj)) {
                                printf("%s << Adding point failed!\n", __FUNCTION__);
                        }
                        
                        //printf("%s << Two projections added.\n", __FUNCTION__);
                } else {
                        //printf("%s << Point already in tracks...\n", __FUNCTION__);
                }
                
                
        }
        
        return sys.tracks.size();
}

void addBlankCamera(SysSBA& sys, cameraParameters& cameraData, bool isFixed) {
        
        frame_common::CamParams cam_params;
        
    cam_params.fx = cameraData.K.at<double>(0, 0);      // Focal length in x
    cam_params.fy = cameraData.K.at<double>(1, 1);      // Focal length in y
    cam_params.cx = cameraData.K.at<double>(0, 2);      // X position of principal point
    cam_params.cy = cameraData.K.at<double>(1, 2);      // Y position of principal point
    cam_params.tx = 0;                                                          // Baseline (no baseline since this is monocular)
    
        Vector4d trans(0, 0, 0, 1);
            
        // Don't rotate.
        Quaterniond rot(1, 0, 0, 0);
        rot.normalize();
        
        //printf("%s << rot = (%f, %f, %f, %f)\n", __FUNCTION__, rot.x(), rot.y(), rot.z(), rot.w());

        // Add a new node to the system.
        sys.addNode(trans, rot, cam_params, isFixed);   // isFixed
        
        if (isFixed) {
                // sys.nFixed++;
        }
        
        /*
        sys.nodes[currIndex].normRot();
        sys.nodes[currIndex].setTransform();
        sys.nodes[currIndex].setProjection();
        sys.nodes[currIndex].setDr(true);
        */

}

void printSystemSummary(SysSBA& sys) {
        printf("%s << sys.nodes.size() = %d\n", __FUNCTION__, sys.nodes.size());
        printf("%s << sys.tracks.size() = %d\n", __FUNCTION__, sys.tracks.size());
}

Quaterniond defaultQuaternion() {
        Quaterniond defQuaternion(1, 0, 0, 0);
        
        return defQuaternion;
}

void updateCameraNode_2(SysSBA& sys, const Mat& C, int image_index) {
        Mat R, t;
        
        Mat Cnew;
        
        if (C.rows == 4) {
                C.copyTo(Cnew);
        } else {
                Cnew = Mat(4, 4, CV_64FC1);
                for (unsigned int iii = 0; iii < 3; iii++) {
                        for (unsigned int jjj = 0; jjj < 4; jjj++) {
                                Cnew.at<double>(iii,jjj) = C.at<double>(iii,jjj);
                                
                        }
                }
                
                Cnew.at<double>(3,0) = 0.0;
                Cnew.at<double>(3,1) = 0.0;
                Cnew.at<double>(3,2) = 0.0;
                Cnew.at<double>(3,3) = 1.0;
        }
        
        Mat C_inv = Cnew.inv();
        
        decomposeTransform(C, R, t);
        
        for (unsigned int iii = 0; iii < 3; iii++) {
                for (unsigned int jjj = 0; jjj < 4; jjj++) {
                        sys.nodes.at(image_index).w2n(iii,jjj) = C.at<double>(iii,jjj);
                }
        }
        
        updateCameraNode_2(sys, R, t, image_index);
}

void updateCameraNode_2(SysSBA& sys, const Mat& R, const Mat& t, int image_index) {
        Vector4d translation;
        Quaterniond rotation;
        
        // printf("%s << t = (%f, %f, %f, %f)\n", __FUNCTION__, t.at<double>(0,0), t.at<double>(1,0), t.at<double>(2,0), t.at<double>(3,0));
        
        translation.x() = t.at<double>(0,0);
        translation.y() = t.at<double>(1,0);
        translation.z() = t.at<double>(2,0);
        
        //printf("%s << Assigning (%d) translation: (%f, %f, %f)\n", __FUNCTION__, image_index, translation.x(), translation.y(), translation.z());
        
        matrixToQuaternion(R, rotation);
        
        //Quaterniond dq = defaultQuaternion();
        //double qa = getQuaternionAngle(rotation, dq);
        
        //printf("%s << Angle = %f / %f\n", __FUNCTION__, qa, qa * 180.0 / M_PI);
        
        //cout << __FUNCTION__ << " << R = " << R << endl;
        
        //printf("%s << rotation = (%f, %f, %f, %f)\n", __FUNCTION__, rotation.x(), rotation.y(), rotation.z(), rotation.w());
        
        Mat R2;
        quaternionToMatrix(rotation, R2);
        
        //cout << __FUNCTION__ << " << R2 = " << R2 << endl;
        
        sys.nodes[image_index].trans = translation;
        sys.nodes[image_index].qrot = rotation;
        
}

void convertProjectionMatCVToEigen(const Mat& mat, Eigen::Matrix< double, 3, 4 > m) {
        
        m(0,0) = mat.at<double>(0,0);
        m(0,1) = mat.at<double>(0,1);
        m(0,2) = mat.at<double>(0,2);
        m(0,3) = mat.at<double>(0,3);
        
        m(1,0) = mat.at<double>(1,0);
        m(1,1) = mat.at<double>(1,1);
        m(1,2) = mat.at<double>(1,2);
        m(1,3) = mat.at<double>(1,3);
        
        m(2,0) = mat.at<double>(2,0);
        m(2,1) = mat.at<double>(2,1);
        m(2,2) = mat.at<double>(2,2);
        m(2,3) = mat.at<double>(2,3);
        
}

void convertProjectionMatEigenToCV(const Eigen::Matrix< double, 3, 4 > m, Mat& mat) {
        
        mat = Mat::zeros(3, 4, CV_64FC1);
        
        mat.at<double>(0,0) = m(0,0);
        mat.at<double>(0,1) = m(0,1);
        mat.at<double>(0,2) = m(0,2);
        mat.at<double>(0,3) = m(0,3);
        
        mat.at<double>(1,0) = m(1,0);
        mat.at<double>(1,1) = m(1,1);
        mat.at<double>(1,2) = m(1,2);
        mat.at<double>(1,3) = m(1,3);
        
        mat.at<double>(2,0) = m(2,0);
        mat.at<double>(2,1) = m(2,1);
        mat.at<double>(2,2) = m(2,2);
        mat.at<double>(2,3) = m(2,3);
        
}

void projectionToRotation(const Mat& src, Mat& dst) {
        dst = Mat(3, 3, CV_64FC1);
        
        dst.at<double>(0,0) = src.at<double>(0,0);
        dst.at<double>(0,1) = src.at<double>(0,1);
        dst.at<double>(0,2) = src.at<double>(0,2);

        dst.at<double>(1,0) = src.at<double>(1,0);
        dst.at<double>(1,1) = src.at<double>(1,1);
        dst.at<double>(1,2) = src.at<double>(1,2);
        
        dst.at<double>(2,0) = src.at<double>(2,0);
        dst.at<double>(2,1) = src.at<double>(2,1);
        dst.at<double>(2,2) = src.at<double>(2,2);
        
}

void rotationToProjection(const Mat& src, Mat& dst) {
        dst = Mat(3, 4, CV_64FC1);
        
        dst.at<double>(0,0) = src.at<double>(0,0);
        dst.at<double>(0,1) = src.at<double>(0,1);
        dst.at<double>(0,2) = src.at<double>(0,2);
        dst.at<double>(0,3) = 0.0;

        dst.at<double>(1,0) = src.at<double>(1,0);
        dst.at<double>(1,1) = src.at<double>(1,1);
        dst.at<double>(1,2) = src.at<double>(1,2);
        dst.at<double>(1,3) = 0.0;
        
        dst.at<double>(2,0) = src.at<double>(2,0);
        dst.at<double>(2,1) = src.at<double>(2,1);
        dst.at<double>(2,2) = src.at<double>(2,2);      
        dst.at<double>(2,3) = 0.0;
        
}

// J.M.P. van Waveren
float ReciprocalSqrt( float x ) {
        long i;
        float y, r;
        
        y = x * 0.5f;
        i = *(long *)( &x );
        i = 0x5f3759df - ( i >> 1 );
        r = *(float *)( &i );
        r = r * ( 1.5f - r * r * y );
        return r;
}

// http://www.euclideanspace.com/maths/geometry/rotations/conversions/matrixToQuaternion/ethan.htm
void matrixToQuaternion(const Mat& mat, Quaterniond& quat) {
        
        // convert mat to Eigen::Matrix3d m;
        /*
        Eigen::Matrix3d m;
        
        m(0,0) = mat.at<double>(0,0);
        m(0,1) = mat.at<double>(0,1);
        m(0,2) = mat.at<double>(0,2);
        
        m(1,0) = mat.at<double>(1,0);
        m(1,1) = mat.at<double>(1,1);
        m(1,2) = mat.at<double>(1,2);
        
        m(2,0) = mat.at<double>(2,0);
        m(2,1) = mat.at<double>(2,1);
        m(2,2) = mat.at<double>(2,2);
        
        quat = Quaterniond(m);
        
        quat.normalize();
        
        return;
                */
        // printf("%s << Entered...\n", __FUNCTION__);
        
        double m00 = mat.at<double>(0,0);
        double m01 = mat.at<double>(0,1);
        double m02 = mat.at<double>(0,2);
        
        double m10 = mat.at<double>(1,0);
        double m11 = mat.at<double>(1,1);
        double m12 = mat.at<double>(1,2);
        
        double m20 = mat.at<double>(2,0);
        double m21 = mat.at<double>(2,1);
        double m22 = mat.at<double>(2,2);
        
        double qw, qx, qy, qz;
        
        
        // ALT 3
        // J.M.P. van Waveren
        /*
        float m[9], q[4];
        m[0] = m00; m[1] = m01; m[2] = m02;
        m[3] = m10; m[4] = m11; m[5] = m12;
        m[6] = m20; m[7] = m21; m[8] = m22;

        //const float *m = jointMats[i].mat;
        if ( m[0 * 4 + 0] + m[1 * 4 + 1] + m[2 * 4 + 2] > 0.0f ) {
                float t = + m[0 * 4 + 0] + m[1 * 4 + 1] + m[2 * 4 + 2] + 1.0f;
                float s = ReciprocalSqrt( t ) * 0.5f;
                q[3] = s * t;
                q[2] = ( m[0 * 4 + 1] - m[1 * 4 + 0] ) * s;
                q[1] = ( m[2 * 4 + 0] - m[0 * 4 + 2] ) * s;
                q[0] = ( m[1 * 4 + 2] - m[2 * 4 + 1] ) * s;
        } else if ( m[0 * 4 + 0] > m[1 * 4 + 1] && m[0 * 4 + 0] > m[2 * 4 + 2] ) {
                float t = + m[0 * 4 + 0] - m[1 * 4 + 1] - m[2 * 4 + 2] + 1.0f;
                float s = ReciprocalSqrt( t ) * 0.5f;
                q[0] = s * t;
                q[1] = ( m[0 * 4 + 1] + m[1 * 4 + 0] ) * s;
                q[2] = ( m[2 * 4 + 0] + m[0 * 4 + 2] ) * s;
                q[3] = ( m[1 * 4 + 2] - m[2 * 4 + 1] ) * s;
        } else if ( m[1 * 4 + 1] > m[2 * 4 + 2] ) {
                float t = - m[0 * 4 + 0] + m[1 * 4 + 1] - m[2 * 4 + 2] + 1.0f;
                float s = ReciprocalSqrt( t ) * 0.5f;
                q[1] = s * t;
                q[0] = ( m[0 * 4 + 1] + m[1 * 4 + 0] ) * s;
                q[3] = ( m[2 * 4 + 0] - m[0 * 4 + 2] ) * s;
                q[2] = ( m[1 * 4 + 2] + m[2 * 4 + 1] ) * s;
        } else {
                float t = - m[0 * 4 + 0] - m[1 * 4 + 1] + m[2 * 4 + 2] + 1.0f;
                float s = ReciprocalSqrt( t ) * 0.5f;
                q[2] = s * t;
                q[3] = ( m[0 * 4 + 1] - m[1 * 4 + 0] ) * s;
                q[0] = ( m[2 * 4 + 0] + m[0 * 4 + 2] ) * s;
                q[1] = ( m[1 * 4 + 2] + m[2 * 4 + 1] ) * s;
        }
        
        qx = (double) q[0];
        qy = (double) q[1];
        qz = (double) q[2];
        qw = (double) q[3];
        */
        
        // ALT 2
        // http://www.koders.com/cpp/fid38F83F165ABF728D6046FD59CF21ECF65E30E2D4.aspx
        /*
        double tmp = abs(m00 + m11 + m22 + 1);
        double s = sqrt(tmp);
        
        if (s > 0.0001) {
                qx = (m21 - m12) / 4*s;
                qy = (m02 - m20) / 4*s;
                qx = (m10 - m01) / 4*s;
        } else {
                int s_iNext[3] = { 2, 3, 1 };
                int i = 1;
                if ( m11 > m00 ) {
                        i = 2;
                }
                
                if ( m22 > m11 ) {
                        i = 3;
                }
                
                int j = s_iNext[i-1];
                int k = s_iNext[j-1];
                
                double fRoot = sqrt(mat.at<double>(i-1,i-1) - mat.at<double>(j-1,j-1) - mat.at<double>(k-1,k-1) + 1.0);
                
                double *tmp[3] = { &qx, &qy, &qz };
                *tmp[i-1] = 0.5 * fRoot;
                s = (mat.at<double>(k-1,j-1)-mat.at<double>(j-1,k-1))*fRoot;
                *tmp[j-1] = (mat.at<double>(j-1,i-1)+mat.at<double>(i-1,j-1))*fRoot;
                *tmp[k-1] = (mat.at<double>(k-1,i-1)+mat.at<double>(i-1,k-1))*fRoot;
        }
        
        if ((qx*qx + qy*qy + qz*qz) != 1.0) {
                qw = sqrt(1 - qx*qx - qy*qy - qz*qz);
        } else {
                qw = 0.0;
        }
        */
        
        // ALT 1
        
        double tr1 = 1.0 + m00 - m11 - m22;
        double tr2 = 1.0 - m00 + m11 - m22;
        double tr3 = 1.0 - m00 - m11 + m22;
        
        
        
        //printf("%s << tr1, tr2, tr3 = (%f, %f, %f)\n", __FUNCTION__, tr1, tr2, tr3);

        if ((tr1 > tr2) && (tr1 > tr3)) { 
                double S = sqrt(tr1) * 2.0; // S=4*qx 
                //printf("%s << Case 1: %f\n", __FUNCTION__, S);
                qw = (m21 - m12) / S;
                qx = 0.25 * S;
                qy = (m01 + m10) / S; 
                qz = (m02 + m20) / S; 
        } else if ((tr2 > tr1) && (tr2 > tr3)) { 
                double S = sqrt(tr2) * 2.0; // S=4*qy
                //printf("%s << Case 2: %f\n", __FUNCTION__, S);
                qw = (m02 - m20) / S;
                qx = (m01 + m10) / S; 
                qy = 0.25 * S;
                qz = (m12 + m21) / S; 
        } else if ((tr3 > tr1) && (tr3 > tr2)) { 
                double S = sqrt(tr3) * 2.0; // S=4*qz
                //printf("%s << Case 3: %f\n", __FUNCTION__, S);
                qw = (m10 - m01) / S;
                qx = (m02 + m20) / S;
                qy = (m12 + m21) / S;
                qz = 0.25 * S;
        } else {
                //printf("%s << Case 4\n", __FUNCTION__);
                qw = 1.0;
                qx = 0.0;
                qy = 0.0;
                qz = 0.0;
        }
        
        
        
        
        
        quat.x() = qx;
        quat.y() = qy;
        quat.z() = qz;
        quat.w() = qw;
        
        //printf("%s << Quat = (%f, %f, %f, %f)\n", __FUNCTION__, qx, qy, qz, qw);
        
        quat.normalize();
        
        //printf("%s << Completed.\n", __FUNCTION__);
}

void updateCameraNode(SysSBA& sys, Mat R, Mat t, int img1, int img2) {
        
        Mat full_R;
        
        Rodrigues(R, full_R);
        
        printf("%s << Trying to determine position of camera node (%d, %d)...\n", __FUNCTION__, img1, img2);
        
        cout << "R = " << R << endl;
        cout << "t = " << t << endl;
        
        // First camera parameters
        Vector4d temptrans = sys.nodes[img1].trans;
        Quaterniond tempqrot = sys.nodes[img1].qrot;
        
        //printf("%s << Debug [%d]\n", __FUNCTION__, 0);
        
        // NEED TO COMBINE TRANSLATION AND QUATERNION FOR PREVIOUS CAMERA WITH THE ONE FOR THE PRESENT ONE...
        // NEED TO KNOW:        CONVERSION FROM R/t to quaternion
        //                                      mapping of one co-ordinate system onto another
        
        // Establish ORIGIN relative to NODE A
                        Vector4d temptrans_N;
                        Quaterniond tempqrot_N;
                        temptrans_N.x() = -temptrans.x();
                        temptrans_N.y() = -temptrans.y();
                        temptrans_N.z() = -temptrans.z();
                        tempqrot_N.x() = tempqrot.x();
                        tempqrot_N.y() = tempqrot.y();
                        tempqrot_N.z() = tempqrot.z();
                        tempqrot_N.w() = -tempqrot.w();
                        tempqrot_N.normalize();
                        
                        sba::Node negativeOrigin;
                        negativeOrigin.trans = temptrans_N;
                        negativeOrigin.qrot = tempqrot_N;
        
        //Mat testMat;
        //Quaterniond testQuaterniond;
        //printf("%s << q(1) = (%f, %f, %f, %f)\n", __FUNCTION__, tempqrot_N.x(), tempqrot_N.y(), tempqrot_N.z(), tempqrot_N.w());
        //convertFromQuaternionToMat(tempqrot_N, testMat);
        //cout << "testMat = " << testMat << endl;
        //convertFromMatToQuaternion(testMat, testQuaterniond);
        //printf("%s << q(2) = (%f, %f, %f, %f)\n", __FUNCTION__, testQuaterniond.x(), testQuaterniond.y(), testQuaterniond.z(), testQuaterniond.w());  
        //convertFromQuaternionToMat(testQuaterniond, testMat);
        //cout << "testMat2 = " << testMat << endl;
        
        //cout << "temptrans_N = " << temptrans_N << endl;
        //cout << "tempqrot_N = " << tempqrot_N << endl;
                        
        //printf("%s << Debug [%d]\n", __FUNCTION__, 1);
                        
        // Establish NODE B relative to NODE A
                                
                        Vector4d temptrans_B;
                        Quaterniond tempqrot_B;
                        temptrans_B.x() = t.at<double>(0,0);
                        temptrans_B.y() = t.at<double>(1,0);
                        temptrans_B.z() = t.at<double>(2,0);
                        
                        matrixToQuaternion(full_R, tempqrot_B);
                        
                        //cout << "full_R = " << full_R << endl;
                        //printf("%s << tempqrot_B = (%f, %f, %f, %f)\n", __FUNCTION__, tempqrot_B.x(), tempqrot_B.y(), tempqrot_B.z(), tempqrot_B.w());
                        
                        //tempqrot_B.x() = R.at<double>(0,0);           // not correct conversion to quaternion
                        //tempqrot_B.y() = R.at<double>(1,0);
                        //tempqrot_B.z() = R.at<double>(2,0);
                        tempqrot_B.normalize();
                        
                        sba::Node secondNode;
                        secondNode.trans = temptrans_B;
                        secondNode.qrot = tempqrot_B;
                        
        //printf("%s << Debug [%d]\n", __FUNCTION__, 2);
        
        // Find transform from Origin to camera B

                        Eigen::Matrix< double, 4, 1 > transformationMat;
                        Eigen::Quaternion< double > quaternionVec;

                        sba::transformN2N(transformationMat, quaternionVec, negativeOrigin, secondNode);
                        
        //printf("%s << Debug [%d]\n", __FUNCTION__, 3);
                        
        // Assign this transformation to the new camera mode
        
                        temptrans.x() = transformationMat(0,0);
                        temptrans.y() = transformationMat(1,0);
                        temptrans.z() = transformationMat(2,0);
                
                        tempqrot.x() = quaternionVec.x();
                        tempqrot.y() = quaternionVec.y();
                        tempqrot.z() = quaternionVec.z();
                        tempqrot.normalize();

                        sys.nodes[img2].trans = temptrans;
                        sys.nodes[img2].qrot = tempqrot;

                        sys.nodes[img2].normRot();
                        sys.nodes[img2].setTransform();
                        sys.nodes[img2].setProjection();
                        sys.nodes[img2].setDr(true);
                        
        //printf("%s << Debug [%d]\n", __FUNCTION__, 4);
        
        return;
        
        // OLD


        // Add the new translations
        temptrans.x() += t.at<double>(0,0);
        temptrans.y() += t.at<double>(1,0);
        temptrans.z() += t.at<double>(2,0);
        
        // This is probably not a valid conversion 
        
        tempqrot.x() += R.at<double>(0,0);
        tempqrot.y() += R.at<double>(1,0);
        tempqrot.z() += R.at<double>(2,0);
        tempqrot.normalize();

        sys.nodes[img2].trans = temptrans;
        sys.nodes[img2].qrot = tempqrot;

        // These methods should be called to update the node.
        
        sys.nodes[img2].normRot();
        sys.nodes[img2].setTransform();
        sys.nodes[img2].setProjection();
        sys.nodes[img2].setDr(true);
        
}

void constrainDodgyPoints(SysSBA& sys) {
        
        for (unsigned int iii = 0; iii < sys.nodes.size(); iii++) {
                if ((abs(sys.nodes[iii].trans.x()) > POSITION_LIMIT) || (abs(sys.nodes[iii].trans.y()) > POSITION_LIMIT) || (abs(sys.nodes[iii].trans.z()) > POSITION_LIMIT)) {
                        sys.nodes[iii].trans.x() = 0.0;
                        sys.nodes[iii].trans.y() = 0.0;
                        sys.nodes[iii].trans.z() = 0.0;
                }
        }
        
        for (unsigned int iii = 0; iii < sys.tracks.size(); iii++) {
                if ((abs(sys.tracks[iii].point.x()) > POSITION_LIMIT) || (abs(sys.tracks[iii].point.y()) > POSITION_LIMIT) || (abs(sys.tracks[iii].point.z()) > POSITION_LIMIT)) {
                        sys.tracks[iii].point.x() = 0.0;
                        sys.tracks[iii].point.y() = 0.0;
                        sys.tracks[iii].point.z() = 0.0;
                }
        }
}

int TriangulateNewTracks(vector<featureTrack>& trackVector, const int index1, const int index2, const Mat& K, const Mat& K_inv, const Mat& P0, const Mat& P1, bool initializeOnFocalPlane) {
        
        Mat coordinateTransform;
        
        int tracksTriangulated = 0;

        
        for (unsigned int iii = 0; iii < trackVector.size(); iii++) {
                
                for (unsigned int jjj = 0; jjj < trackVector.at(iii).locations.size()-1; jjj++) {
                                        
                        int trInd1 = trackVector.at(iii).locations.at(jjj).imageIndex;
                                
                        if (trInd1 == index1) {
                                
                                for (unsigned int kkk = jjj+1; kkk < trackVector.at(iii).locations.size(); kkk++) {
                                        
                                        int trInd2 = trackVector.at(iii).locations.at(kkk).imageIndex;
                                        
                                        if (trInd2 == index2) {
                                                
                                                Point2f loc1 = trackVector.at(iii).locations.at(jjj).featureCoord;
                                                Point2f loc2 = trackVector.at(iii).locations.at(kkk).featureCoord;
                                                
                                                Point3d xyzPoint;
                        
                                                // This puts the 3D point out but in camera A's co-ordinates
                                                if (initializeOnFocalPlane) {
                                                        xyzPoint.x = loc1.x;
                                                        xyzPoint.y = loc1.y;
                                                        xyzPoint.z = 1.0;
                                                } else {
                                                        Triangulate(loc1, loc2, K, K_inv, P0, P1, xyzPoint);
                                                }
                                                
                                                //printf("%s << PT relative to cam A: (%f, %f, %f)\n", __FUNCTION__, xyzPoint.x, xyzPoint.y, xyzPoint.z);
                                                
                                                // Convert XYZ co-ordinate to WORLD co-ordinates
                                                
                                                trackVector.at(iii).set3dLoc(xyzPoint);
                                                //trackVector.at(iii).xyzEstimate.x = xyzPoint.x;
                                                //trackVector.at(iii).xyzEstimate.y = xyzPoint.y;
                                                //trackVector.at(iii).xyzEstimate.z = xyzPoint.z;
                                                
                                                //printf("%s << PT relative to WORLD: (%f, %f, %f)\n", __FUNCTION__, trackVector.at(iii).xyzEstimate.x, trackVector.at(iii).xyzEstimate.y, trackVector.at(iii).xyzEstimate.z);
                                                
                                                tracksTriangulated++;
                                                
                                        }
                                        
                                }
                                
                        }
                                
                                
                }
                
                
                
        
        }
        
        return tracksTriangulated;

}

void ExtractPointCloud(vector<Point3d>& cloud, vector<featureTrack>& trackVector) {
        
        
        for (unsigned int iii = 0; iii < trackVector.size(); iii++) {
                // Copy the track vector co-ordinate
                cloud.push_back(trackVector.at(iii).get3dLoc());
        }
        
        
}



void reduceVectorsToTrackedPoints(const vector<Point2f>& points1, vector<Point2f>& trackedPoints1, const vector<Point2f>& points2, vector<Point2f>& trackedPoints2, vector<uchar>& statusVec) {
        
        trackedPoints1.clear();
        trackedPoints2.clear();
        
        for (unsigned int iii = 0; iii < statusVec.size(); iii++) {
                
                if (statusVec.at(iii) > 0) {
                        //printf("%s << 1: statusVec.at(%d) = %c\n", __FUNCTION__, iii, statusVec.at(iii));
                        trackedPoints1.push_back(points1.at(iii));
                        trackedPoints2.push_back(points2.at(iii));
                } else {
                        //printf("%s << 0: statusVec.at(%d) = %c\n", __FUNCTION__, iii, statusVec.at(iii));
                }
        }
        
}

bool findBestReconstruction(const Mat& P0, Mat& P1, Mat& R, Mat& t, const SVD& svd, const Mat& K, const vector<Point2f>& pts1, const vector<Point2f>& pts2, bool useDefault) {
        
        bool validity = false;
        
        //printf("%s << Entered.\n", __FUNCTION__);
        
        if (pts1.size() != pts2.size()) {
                printf("%s << ERROR! Point vector size mismatch.\n", __FUNCTION__);
        }
        
        int bestIndex = 0, bestScore = -1;
        Mat t33, t_tmp[4], R_tmp[4], W, Z, Winv, I, negI, P1_tmp[4];
        Mat Kinv;
        //printf("%s << DEBUG [%d].\n", __FUNCTION__, -2);
        //printf("%s << DEBUG [%d].\n", __FUNCTION__, -1);
        Kinv = K.inv();
        
        //printf("%s << DEBUG [%d].\n", __FUNCTION__, 0);
        
        Mat zeroTrans;
        zeroTrans = Mat::zeros(3, 1, CV_64F);   // correct dimension?
        
        I = Mat::eye(3, 3, CV_64FC1);
        negI = -I;
        getWandZ(W, Winv, Z);
        
        //printf("%s << DEBUG [%d].\n", __FUNCTION__, 1);
        
        Mat u_transpose;
        transpose(svd.u, u_transpose);
        
        Mat rotation1;
        Rodrigues(I, rotation1);
        
        //printf("%s << DEBUG [%d].\n", __FUNCTION__, 2);

        //cout << "rotation1 = " << rotation1 << endl;
        
        float sig = rotation1.at<double>(0,1);
        float phi = rotation1.at<double>(0,2);

        //printf("%s << sig = %f; phi = %f\n", __FUNCTION__, sig, phi);

        Point3d unitVec1(1.0*sin(sig)*cos(phi), 1.0*sin(sig)*cos(phi), 1.0*cos(sig));
        
        //printf("%s << DEBUG [%d].\n", __FUNCTION__, 3);

        Mat uv1(unitVec1);
        
        //printf("%s << DEBUG [%d].\n", __FUNCTION__, 4);
        
        // Go through all four cases...
        for (int zzz = 0; zzz < 4; zzz++) {
                
                //printf("%s << Loop [%d]\n", __FUNCTION__, zzz);
                
                switch (zzz) {
                        case 0:
                                //t33 = v * W * SIGMA * vt;
                                t33 = svd.u * Z * u_transpose;
                                R_tmp[zzz] = svd.u * Winv * svd.vt;
                                break;
                        case 1:
                                //t33 = v * W_inv * SIGMA * vt;
                                t33 = negI * svd.u * Z * u_transpose;
                                R_tmp[zzz] = svd.u * Winv * svd.vt;
                                break;
                        case 2:
                                //t33 = v * W * SIGMA * vt;
                                t33 = svd.u * Z * u_transpose;
                                R_tmp[zzz] = svd.u * W * svd.vt;
                                break;
                        case 3:
                                //t33 = v * W_inv * SIGMA * vt;
                                t33 = negI * svd.u * Z * u_transpose;
                                R_tmp[zzz] = svd.u * W * svd.vt;
                                break;
                        default:
                                break;
                }
                
                // Converting 3x3 t mat to vector
                t_tmp[zzz] = Mat::zeros(3, 1, CV_64F);
                t_tmp[zzz].at<double>(0,0) = -t33.at<double>(1,2);
                t_tmp[zzz].at<double>(1,0) = t33.at<double>(0,2);
                t_tmp[zzz].at<double>(2,0) = -t33.at<double>(0,1);
                
                // Get new P1 matrix
                findP1Matrix(P1_tmp[zzz], R_tmp[zzz], t_tmp[zzz]);
                
                // Find camera vectors
                Mat rotation2;
                Rodrigues(R_tmp[zzz], rotation2);

                //cout << "rotation2 = " << rotation2 << endl;

                sig = rotation2.at<double>(0,1);
                phi = rotation2.at<double>(0,2);

                //printf("%s << sig = %f; phi = %f\n", __FUNCTION__, sig, phi);

                Point3d unitVec2(1.0*sin(sig)*cos(phi), 1.0*sin(sig)*cos(phi), 1.0*cos(sig));

                Mat uv2(unitVec2);

                //printf("%s << unitVec2 = (%f, %f, %f)\n", __FUNCTION__, unitVec2.x, unitVec2.y, unitVec2.z);
                
                int badCount = 0;
                
                vector<Point3d> pointcloud;
                vector<Point2f> correspImg1Pt;
                
                //printf("%s << DEBUG [%d].\n", __FUNCTION__, 7);
                
                TriangulatePoints(pts1, pts2, K, Kinv, P0, P1_tmp[zzz], pointcloud, correspImg1Pt);
                
                //printf("%s << DEBUG [%d]. (pointcloud.size() = %d)\n", __FUNCTION__, 8, pointcloud.size());
                
                Mat Rs_k[2], ts_k[2];

                // Then check how many points are NOT in front of BOTH of the cameras
                for (unsigned int kkk = 0; kkk < pointcloud.size(); kkk++) {
                        
                        
                        
                        //printf("%s << DEBUG [%d][%d].\n", __FUNCTION__, kkk, 0);

                        Point3d rayVec1, rayVec2;

                        Rs_k[0] = I;
                        Rs_k[1] = R_tmp[zzz];

                        ts_k[0] = zeroTrans;
                        ts_k[1] = t_tmp[zzz];
                        
                        //printf("%s << DEBUG [%d][%d].\n", __FUNCTION__, kkk, 1);
                        
                        //cout << endl << "t_tmp[zzz] = " << t_tmp[zzz] << endl;

                        //printf("%s << objpt = (%f, %f, %f)\n", __FUNCTION__, objpt.x, objpt.y, objpt.z);

                        // so this is getting the displacements between the points and both cameras...
                        rayVec1.x = pointcloud.at(kkk).x - ts_k[0].at<double>(0,0);
                        rayVec1.y = pointcloud.at(kkk).y - ts_k[0].at<double>(1,0);
                        rayVec1.z = pointcloud.at(kkk).z - ts_k[0].at<double>(2,0);

                        rayVec2.x = pointcloud.at(kkk).x - ts_k[1].at<double>(0,0);
                        rayVec2.y = pointcloud.at(kkk).y - ts_k[1].at<double>(1,0);
                        rayVec2.z = pointcloud.at(kkk).z - ts_k[1].at<double>(2,0);
                        
                        /*
                        if (kkk < 0) {
                                printf("%s << pt(%d) = (%f, %f, %f)\n", __FUNCTION__, kkk, pointcloud.at(kkk).x, pointcloud.at(kkk).y, pointcloud.at(kkk).z);
                                printf("%s << rayVec1(%d) = (%f, %f, %f)\n", __FUNCTION__, kkk, rayVec1.x, rayVec1.y, rayVec1.z);
                                printf("%s << rayVec2(%d) = (%f, %f, %f)\n", __FUNCTION__, kkk, rayVec2.x, rayVec2.y, rayVec2.z);
                        }
                        * */
                        
                        //printf("%s << DEBUG [%d][%d].\n", __FUNCTION__, kkk, 2);

                        //printf("%s << rayVec1 = (%f, %f, %f)\n", __FUNCTION__, rayVec1.x, rayVec1.y, rayVec1.z);
                        //printf("%s << rayVec2 = (%f, %f, %f)\n", __FUNCTION__, rayVec2.x, rayVec2.y, rayVec2.z);

                        //cin.get();

                        double dot1, dot2;
                        
                        Mat rv1(rayVec1), rv2(rayVec2);
                        
                        /*
                        cout << endl << "uv1 = " << uv1 << endl;
                        cout << endl << "rv1 = " << rv1 << endl;
                        
                        cout << endl << "uv2 = " << uv2 << endl;
                        cout << endl << "rv2 = " << rv2 << endl;
                        */

                        dot1 = uv1.dot(rv1);
                        dot2 = uv2.dot(rv2);

                        //printf("%s << DEBUG [%d][%d].\n", __FUNCTION__, kkk, 3);

                        //printf("%s << dot1 = %f; dot2 = %f\n", __FUNCTION__, dot1, dot2);

                        double mag1 = pow(pow(rayVec1.x, 2) + pow(rayVec1.y, 2) + pow(rayVec1.z, 2), 0.5);
                        double mag2 = pow(pow(rayVec2.x, 2) + pow(rayVec2.y, 2) + pow(rayVec2.z, 2), 0.5);

                        //printf("%s << mag1 = %f; mag2 = %f\n", __FUNCTION__, mag1, mag2);

                        double ang1 = fmod(acos(dot1 / mag1), 2*M_PI);
                        double ang2 = fmod(acos(dot2 / mag2), 2*M_PI);

                        //printf("%s << ang1 = %f; ang2 = %f\n", __FUNCTION__, ang1, ang2);
                        // just try to check if angle is greater than 90deg from where each camera is facing to the pt

                        // get vector from each camera to the point

                        // use dot product to determine angle
                        if ((abs(ang1) < M_PI/2 ) || (abs(ang2) < M_PI/2)) {
                                //printf("%s << ang1 = %f; ang2 = %f\n", __FUNCTION__, ang1, ang2);
                        //if ((dot1 < 0.0) || (dot2 < 0.0)) {
                                validity = false;
                                badCount++;
                        }

                        if (rayVec1.z < 0) {
                                //printf("%s << rayVec1.z < 0; abs(ang1) = %f\n", __FUNCTION__, abs(ang1));

                        } else {
                                //printf("%s << rayVec1.z >= 0; abs(ang1) = %f\n", __FUNCTION__, abs(ang1));

                        }

                        //cin.get();

                }

                printf("%s << badCount[%d] = %d\n", __FUNCTION__, zzz, badCount);
                
                if (zzz == 0) {
                        bestScore = badCount;
                } else {
                        if (badCount < bestScore) {
                                bestScore = badCount;
                                bestIndex = zzz;
                        }
                }

                
        }
        
        if (bestScore == 0) {
                validity = true;
        }
        
        // Should be 2 - but try different ones...
        if (useDefault) {
                bestIndex = 2;
                printf("%s << Defaulting to model #%d\n", __FUNCTION__, bestIndex);
        }
        
        findP1Matrix(P1, R_tmp[bestIndex], t_tmp[bestIndex]);

        R_tmp[bestIndex].copyTo(R);
        t_tmp[bestIndex].copyTo(t);
        
        return validity;
}

void initializeP0(Mat& P) {
        P = Mat::zeros(3, 4, CV_64FC1);
        P.at<double>(0,0) = 1.0;
        P.at<double>(0,1) = 0.0;
        P.at<double>(0,2) = 0.0;
        P.at<double>(0,3) = 0.0;
        P.at<double>(1,0) = 0.0;
        P.at<double>(1,1) = 1.0;
        P.at<double>(1,2) = 0.0;
        P.at<double>(1,3) = 0.0;
        P.at<double>(2,0) = 0.0;
        P.at<double>(2,1) = 0.0;
        P.at<double>(2,2) = 1.0;
        P.at<double>(2,3) = 0.0;
}

void LinearLSTriangulation(Mat& dst, const Point3d& u, const Mat& P, const Point3d& u1, const Mat& P1) {
//homogenous image point (u,v,1)
//camera 1 matrix
//homogenous image point in 2nd camera
//camera 2 matrix
        
        
    //build matrix A for homogenous equation system Ax = 0
    //assume X = (x,y,z,1), for Linear-LS method
    //which turns it into a AX = B system, where A is 4x3, X is 3x1 and B is 4x1
    
    Mat A(4, 3, CV_64FC1);
    
    A.at<double>(0,0) = (double) u.x * P.at<double>(2,0) - P.at<double>(0,0);
    A.at<double>(0,1) = (double) u.x * P.at<double>(2,1) - P.at<double>(0,1);
    A.at<double>(0,2) = (double) u.x * P.at<double>(2,2) - P.at<double>(0,2);
    
    A.at<double>(1,0) = (double) u.y * P.at<double>(2,0) - P.at<double>(1,0);
    A.at<double>(1,1) = (double) u.y * P.at<double>(2,1) - P.at<double>(1,1);
    A.at<double>(1,2) = (double) u.y * P.at<double>(2,2) - P.at<double>(1,2);
    
    A.at<double>(2,0) = (double) u1.x * P1.at<double>(2,0) - P1.at<double>(0,0);
    A.at<double>(2,1) = (double) u1.x * P1.at<double>(2,1) - P1.at<double>(0,1);
    A.at<double>(2,2) = (double) u1.x * P1.at<double>(2,2) - P1.at<double>(0,2);
    
    A.at<double>(3,0) = (double) u1.y * P1.at<double>(2,0) - P1.at<double>(1,0);
    A.at<double>(3,1) = (double) u1.y * P1.at<double>(2,1) - P1.at<double>(1,1);
    A.at<double>(3,2) = (double) u1.y * P1.at<double>(2,2) - P1.at<double>(1,2);

        Mat B(4, 1, CV_64FC1);
        
        B.at<double>(0,0) = -((double) u.x * P.at<double>(2,3) - P.at<double>(0,3));
        B.at<double>(0,1) = -((double) u.y * P.at<double>(2,3) - P.at<double>(1,3));
        B.at<double>(0,2) = -((double) u1.x * P1.at<double>(2,3) - P1.at<double>(0,3));
        B.at<double>(0,3) = -((double) u1.y * P1.at<double>(2,3) - P1.at<double>(1,3));
 
    solve(A, B, dst, DECOMP_SVD);

}

void IterativeLinearLSTriangulation(Mat& dst, const Point3d& u, const Mat&  P, const Point3d& u1, const Mat&  P1) {
        
    double wi = 1, wi1 = 1;
    int maxIterations = 10; //Hartley suggests 10 iterations at most
    
    Mat X(4, 1, CV_64FC1), X_;
    
    LinearLSTriangulation(X_, u, P, u1, P1);
        
        X.at<double>(0,0) = X_.at<double>(0,0);
        X.at<double>(1,0) = X_.at<double>(1,0); 
        X.at<double>(2,0) = X_.at<double>(2,0); 
        X.at<double>(3,0) = 1.0; 
    
    for (int i=0; i<maxIterations; i++) {
 
        //recalculate weights
        Mat p2x_m = P.row(2) * X;
        double p2x = p2x_m.at<double>(0, 0);
        
        Mat p2x1_m = P1.row(2) * X;
        double p2x1 = p2x1_m.at<double>(0, 0);
        
        //printf("%s << %f, %f, EPSILON = %f\n", __FUNCTION__, fabsf(wi - p2x), fabsf(wi1 - p2x1), EPSILON);
 
        //breaking point
        if ((fabsf(wi - p2x) <= EPSILON) && (fabsf(wi1 - p2x1) <= EPSILON)) {
                        //printf("%s << About to break...\n", __FUNCTION__);
                        //cin.get();
                        break;
                } 
 
        wi = p2x;
        wi1 = p2x1;
 
        //reweight equations and solve
        Mat A(4, 3, CV_64FC1);
        
        A.at<double>(0, 0) = (u.x * P.at<double>(2,0) - P.at<double>(0,0))/wi;
        A.at<double>(0, 1) = (u.x*P.at<double>(2,1)-P.at<double>(0,1))/wi;
        A.at<double>(0, 2) = (u.x*P.at<double>(2,2)-P.at<double>(0,2))/wi;
        
        A.at<double>(1, 0) = (u.y*P.at<double>(2,0)-P.at<double>(1,0))/wi;
        A.at<double>(1, 1) = (u.y*P.at<double>(2,1)-P.at<double>(1,1))/wi;
        A.at<double>(1, 2) = (u.y*P.at<double>(2,2)-P.at<double>(1,2))/wi;
        
        A.at<double>(2, 0) = (u1.x*P1.at<double>(2,0)-P1.at<double>(0,0))/wi1;
        A.at<double>(2, 1) = (u1.x*P1.at<double>(2,1)-P1.at<double>(0,1))/wi1;
        A.at<double>(2, 2) = (u1.x*P1.at<double>(2,2)-P1.at<double>(0,2))/wi1;
        
        A.at<double>(3, 0) = (u1.y*P1.at<double>(2,0)-P1.at<double>(1,0))/wi1;
        A.at<double>(3, 1) = (u1.y*P1.at<double>(2,1)-P1.at<double>(1,1))/wi1;
        A.at<double>(3, 2) = (u1.y*P1.at<double>(2,2)-P1.at<double>(1,2))/wi1;
        
        Mat B(4, 1, CV_64FC1);
        
        B.at<double>(0, 0) = -(u.x*P.at<double>(2,3)    -P.at<double>(0,3))/wi;
        B.at<double>(1, 0) = -(u.y*P.at<double>(2,3)  -P.at<double>(1,3))/wi;
        B.at<double>(2, 0) = -(u1.x*P1.at<double>(2,3)    -P1.at<double>(0,3))/wi1;
        B.at<double>(3, 0) = -(u1.y*P1.at<double>(2,3)    -P1.at<double>(1,3))/wi1;
        
        //printf("%s << Solving decomposition...\n", __FUNCTION__);
 
        solve(A, B, X_, DECOMP_SVD);
        
        //printf("%s << Decomposition solved.\n", __FUNCTION__);
        
        X.at<double>(0,0) = X_.at<double>(0,0);
        X.at<double>(1,0) = X_.at<double>(1,0); 
        X.at<double>(2,0) = X_.at<double>(2,0); 
        X.at<double>(3,0) = 1.0; 
        
    }
    
    X.copyTo(dst);

}

void getWandZ(Mat& W, Mat& Winv, Mat& Z) {
        W = Mat::zeros(3, 3, CV_64FC1);
        
        W.at<double>(0,0) = 0.0;
        W.at<double>(0,1) = -1.0;
        W.at<double>(0,2) = 0.0;
        
        W.at<double>(1,0) = 1.0;
        W.at<double>(1,1) = 0.0;
        W.at<double>(1,2) = 0.0;
        
        W.at<double>(2,0) = 0.0;
        W.at<double>(2,1) = 0.0;
        W.at<double>(2,2) = 1.0;
        
        Winv = Mat::zeros(3, 3, CV_64FC1);
        
        Winv.at<double>(0,0) = 0.0;
        Winv.at<double>(0,1) = 1.0;
        Winv.at<double>(0,2) = 0.0;
        
        Winv.at<double>(1,0) = -1.0;
        Winv.at<double>(1,1) = 0.0;
        Winv.at<double>(1,2) = 0.0;
        
        Winv.at<double>(2,0) = 0.0;
        Winv.at<double>(2,1) = 0.0;
        Winv.at<double>(2,2) = 1.0;     
        
        Z = Mat::zeros(3, 3, CV_64FC1);
        
        Z.at<double>(0,1) = 1.0;
        Z.at<double>(1,0) = -1.0;
}

void findP1Matrix(Mat& P1, const Mat& R, const Mat& t) {
        //Mat W, Winv, Z;
        
        //getWandZ(W, Z);
        //Winv = W.inv();

        //R = svd.u * W * svd.vt; //HZ 9.19
        //t = svd.u.col(2); //u3
        
        P1 = Mat(3, 4, CV_64FC1);
                        
        P1.at<double>(0,0) = R.at<double>(0,0);
        P1.at<double>(0,1) = R.at<double>(0,1);
        P1.at<double>(0,2) = R.at<double>(0,2);
        P1.at<double>(0,3) = t.at<double>(0,0);
        
        P1.at<double>(1,0) = R.at<double>(1,0);
        P1.at<double>(1,1) = R.at<double>(1,1);
        P1.at<double>(1,2) = R.at<double>(1,2);
        P1.at<double>(1,3) = t.at<double>(1,0);
        
        P1.at<double>(2,0) = R.at<double>(2,0);
        P1.at<double>(2,1) = R.at<double>(2,1);
        P1.at<double>(2,2) = R.at<double>(2,2);
        P1.at<double>(2,3) = t.at<double>(2,0);
}

void Triangulate(const Point2f& pt1, const Point2f& pt2, const Mat& K, const Mat& Kinv, const Mat& P1, const Mat& P2, Point3d& xyzPoint, bool debug) {

        Mat C1, C2;
        projectionToTransformation(P1, C1);
        projectionToTransformation(P2, C2);
        
        // Relative
        Mat P0, PR, C0, CR;
        initializeP0(P0);
        projectionToTransformation(P0, C0);
        CR = C2 * C1.inv();
        transformationToProjection(CR, PR);

        Point3d xyzPoint_R;

        if (debug) {
                cout << __FUNCTION__ << " << Provided = " << endl;
                cout << "P1 = " << P1 << endl;
                cout << "P2 = " << P2 << endl;
        }
        
        Point2f kp = pt1;
        Point3d u(kp.x, kp.y, 1.0);
        
        Mat um = Kinv * Mat(u);
        
        u.x = um.at<double>(0, 0);
        u.y = um.at<double>(1, 0);
        u.z = um.at<double>(2, 0);
        
        Point2f kp1 = pt2;
        Point3d u1(kp1.x, kp1.y,1.0);
        Mat um1 = Kinv * Mat(u1);
        
        u1.x = um1.at<double>(0, 0);
        u1.y = um1.at<double>(1, 0);
        u1.z = um1.at<double>(2, 0);
        
        if (debug) {
                cout << __FUNCTION__ << " << Used = " << endl;
                cout << "P0 = " << P0 << endl;
                cout << "PR = " << PR << endl;
        }
        
        Mat X;
        //LinearLSTriangulation(X, u, P0, u1, PR);
        IterativeLinearLSTriangulation(X, u, P0, u1, PR);       // switched from u then u1
        
        xyzPoint_R = Point3d( X.at<double>(0, 0), X.at<double>(1, 0), X.at<double>(2, 0) );
        
        Mat A(4, 1, CV_64FC1), B(4, 1, CV_64FC1);
        
        A.at<double>(0,0) = xyzPoint_R.x;
        A.at<double>(1,0) = xyzPoint_R.y;
        A.at<double>(2,0) = xyzPoint_R.z;
        A.at<double>(3,0) = 1.0;
        
        B = C1 * A;
        
        xyzPoint.x = B.at<double>(0,0) / B.at<double>(3,0);
        xyzPoint.y = B.at<double>(1,0) / B.at<double>(3,0);
        xyzPoint.z = B.at<double>(2,0) / B.at<double>(3,0);
        
        if (debug) {
                printf("%s << u = (%f, %f, %f)\n", __FUNCTION__, u.x, u.y, u.z);
                printf("%s << u1 = (%f, %f, %f)\n", __FUNCTION__, u1.x, u1.y, u1.z);
                printf("%s << xyzPoint_R = (%f, %f, %f)\n", __FUNCTION__, xyzPoint_R.x, xyzPoint_R.y, xyzPoint_R.z);
                printf("%s << xyzPoint = (%f, %f, %f)\n", __FUNCTION__, xyzPoint.x, xyzPoint.y, xyzPoint.z);
                cin.get();
        }
        

}

void TriangulatePoints(const vector<Point2f>& pt_set1,
                       const vector<Point2f>& pt_set2,
                       const Mat& K,
                       const Mat& Kinv,
                       const Mat& P,
                       const Mat& P1,
                       vector<Point3d>& pointcloud,
                       vector<Point2f>& correspImg1Pt)
{
 
    pointcloud.clear();
    correspImg1Pt.clear();
    
    Point3d point_3;

    for (unsigned int iii = 0; iii < pt_set1.size(); iii++) {    
                
                Triangulate(pt_set1.at(iii), pt_set2.at(iii), K, Kinv,  P, P1, point_3, false);
                 
                pointcloud.push_back(point_3);
                correspImg1Pt.push_back(pt_set1[iii]);
        }
}

void findCentroid(vector<featureTrack>& tracks, Point3d& centroid, Point3d& stdDeviation) {
        
        unsigned int triangulatedPoints = 0;
        
        for (unsigned int iii = 0; iii < tracks.size(); iii++) {
                if (tracks.at(iii).isTriangulated) {
                        triangulatedPoints++;
                }
        }
        
        printf("%s << triangulatedPoints = %d\n", __FUNCTION__, triangulatedPoints);
        
        centroid = Point3d(0.0, 0.0, 0.0);
        stdDeviation = Point3d(0.0, 0.0, 0.0);
        
        for (unsigned int iii = 0; iii < tracks.size(); iii++) {
                if (tracks.at(iii).isTriangulated) {
                        centroid.x += (tracks.at(iii).get3dLoc().x / ((double) triangulatedPoints));
                        centroid.y += (tracks.at(iii).get3dLoc().y / ((double) triangulatedPoints));
                        centroid.z += (tracks.at(iii).get3dLoc().z / ((double) triangulatedPoints));
                }
        }
        
        for (unsigned int iii = 0; iii < tracks.size(); iii++) {
                if (tracks.at(iii).isTriangulated) {
                        stdDeviation.x += (pow((tracks.at(iii).get3dLoc().x - centroid.x), 2.0) / ((double) triangulatedPoints));
                        stdDeviation.y += (pow((tracks.at(iii).get3dLoc().y - centroid.y), 2.0) / ((double) triangulatedPoints));
                        stdDeviation.z += (pow((tracks.at(iii).get3dLoc().z - centroid.z), 2.0) / ((double) triangulatedPoints));
                }
                
        }
        
        stdDeviation.x = pow(stdDeviation.x, 0.5);
        stdDeviation.y = pow(stdDeviation.y, 0.5);
        stdDeviation.z = pow(stdDeviation.z, 0.5);
        
}

---

thermalvis
Author(s): Stephen Vidas
autogenerated on Tue Mar 5 12:25:24 2013