$search

tracks.cpp

Go to the documentation of this file.

#include "tracks.hpp"

featureTrack::featureTrack() {
        isTriangulated = false;
}

void featureTrack::set3dLoc(const Point3d& loc) {
        
        if (!isTriangulated) {
                xyzEstimate.x = loc.x;
                xyzEstimate.y = loc.y;
                xyzEstimate.z = loc.z;
        
                isTriangulated = true;
        }
        
}

void featureTrack::reset3dLoc(const Point3d& loc) {
        
        xyzEstimate.x = loc.x;
        xyzEstimate.y = loc.y;
        xyzEstimate.z = loc.z;

        isTriangulated = true;
        
}

Point2f featureTrack::getCoord(unsigned int cam_idx) {
        
        Point2f blankPt;
        
        for (unsigned int iii = 0; iii < locations.size(); iii++) {
                
                if (((unsigned int) locations.at(iii).imageIndex) == cam_idx) {
                        blankPt.x = locations.at(iii).featureCoord.x;
                        blankPt.y = locations.at(iii).featureCoord.y;
                        
                        return blankPt;
                }
                
                
        }
        
}

Point3d featureTrack::get3dLoc() {
        
        Point3d loc;
        
        loc.x = xyzEstimate.x;
        loc.y = xyzEstimate.y;
        loc.z = xyzEstimate.z;
        
        return loc;
}

void addMatchToVector(vector<featureTrack>& featureTrackVector, int index1, const Point2f& point1, int index2, const Point2f& point2) {
        
        // Go through all of the feature tracks to check if either of these features exist.
        
        bool everFoundFeature1or2 = false;

        // For each track
        for (unsigned int iii = 0; iii < featureTrackVector.size(); iii++) {
                
                bool foundFeature1 = false, foundFeature2 = false;
                bool foundImage1 = false, foundImage2 = false;
                
                // int foundIndex1 = -1, foundIndex2 = -1;
                
                
                
                // For each existing feature in the track
                for (unsigned int jjj = 0; jjj < featureTrackVector.at(iii).locations.size(); jjj++) {
                        
                        // If you've found a feature from the correct image
                        if (featureTrackVector.at(iii).locations.at(jjj).imageIndex == index1) {
                                foundImage1 = true;
                                if (featureTrackVector.at(iii).locations.at(jjj).featureCoord == point1) {
                                        foundFeature1 = true;
                                        //foundIndex1 = jjj;
                                }
                        }
                        
                        if (featureTrackVector.at(iii).locations.at(jjj).imageIndex == index2) {
                                foundImage2 = true;
                                if (featureTrackVector.at(iii).locations.at(jjj).featureCoord == point2) {
                                        foundFeature2 = true;
                                        //foundIndex2 = jjj;
                                }
                        }
                        
                }
                
                if (foundFeature1 && foundFeature2) {
                        everFoundFeature1or2 = true;
                } else if (foundFeature1) {
                        everFoundFeature1or2 = true;
                        // add feature 2 if no feature from this image in track
                        if (!foundImage2) {
                                indexedFeature featToAdd(index2, point2);
                                featureTrackVector.at(iii).addFeature(featToAdd);
                        }
                } else if (foundFeature2) {
                        everFoundFeature1or2 = true;
                        // add feature 1 if no feature from this image in track
                        if (!foundImage1) {
                                indexedFeature featToAdd(index1, point1);
                                featureTrackVector.at(iii).addFeature(featToAdd);
                        }
                        
                }
                
        }
        
        if (!everFoundFeature1or2) {
                // If neither does, create a new track
                featureTrack newFeatureTrack;
                indexedFeature featToAdd1(index1, point1);
                newFeatureTrack.addFeature(featToAdd1);
                indexedFeature featToAdd2(index2, point2);
                newFeatureTrack.addFeature(featToAdd2);
                featureTrackVector.push_back(newFeatureTrack);
        }
        
                        
}

void drawFeatureTracks(Mat& src, Mat& dst, vector<featureTrack>& tracks, const Scalar& tColor, const Scalar& kColor, int index, unsigned int history) {
        
        cv::Point p1, p2;
        
        src.copyTo(dst);
        
        
        
        for (unsigned int iii = 0; iii < tracks.size(); iii++) {
                
                //printf("%s << DEBUG [%d] : %d\n", __FUNCTION__, iii, 0);
                
                bool trackFound = false;
                int latestIndex = 0;
                
                for (unsigned int jjj = tracks.at(iii).locations.size()-1; jjj > ((unsigned int) std::max((latestIndex-((int) history)), 0)); jjj--) {
                        
                        //printf("%s << DEBUG [%d][%d] : %d\n", __FUNCTION__, iii, jjj, 0);
                        
                        if (trackFound) {
                                // Draw line between points...
                                
                                p1 = cv::Point(tracks.at(iii).locations.at(jjj+1).featureCoord.x * 16.0, tracks.at(iii).locations.at(jjj+1).featureCoord.y * 16.0);
                                p2 = cv::Point(tracks.at(iii).locations.at(jjj).featureCoord.x * 16.0, tracks.at(iii).locations.at(jjj).featureCoord.y * 16.0);
                
                                cv::line(dst, p1, p2, tColor, 0.5, CV_AA, 4);
                                
                        } else {
                                latestIndex = jjj;
                        }
                        
                        //printf("%s << DEBUG [%d][%d] : %d\n", __FUNCTION__, iii, jjj, 1);
                        
                        if (tracks.at(iii).locations.at(jjj).imageIndex == index) {
                                trackFound = true;
                                p1 = cv::Point(tracks.at(iii).locations.at(jjj).featureCoord.x * 16.0, tracks.at(iii).locations.at(jjj).featureCoord.y * 16.0);
                                circle(dst, p1, 1, kColor, 2, CV_AA, 4);
                        }
                        
                        //printf("%s << DEBUG [%d][%d] : %d\n", __FUNCTION__, iii, jjj, 2);
                
                }
                
        }
        
        
}

void redistortTracks(const vector<featureTrack>& src, vector<featureTrack>& dst, const Mat& cameraMatrix, const Mat& distCoeffs, unsigned int latest, const Mat& newCamMat, unsigned int history) {
        
        dst.clear();
        dst.insert(dst.end(), src.begin(), src.end());
        
        vector<Point2f> tempPoints, newPoints;
        
        
        for (unsigned int iii = 0; iii < src.size(); iii++) {
                
                if (src.at(iii).locations.at(src.at(iii).locations.size()-1).imageIndex != ((int) latest)) {
                        continue;
                }
                
                //printf("%s << continuing...\n", __FUNCTION__);
                
                newPoints.clear();
                tempPoints.clear();
                
                for (int jjj = src.at(iii).locations.size()-1; jjj >= ((int) std::max(((int) src.at(iii).locations.size()) - ((int) history), ((int) 0))); jjj--) {
                        
                        //printf("%s << A [%d] (%d / %d)\n", __FUNCTION__, iii, jjj, src.at(iii).locations.size());
                        
                        tempPoints.push_back(src.at(iii).locations.at(jjj).featureCoord);
                } 
                
                redistortPoints(tempPoints, newPoints, cameraMatrix, distCoeffs, newCamMat);
                
                unsigned int currIndex = 0;
                
                for (int jjj = src.at(iii).locations.size()-1; jjj >= ((int) std::max(((int) src.at(iii).locations.size()) - ((int) history), ((int) 0))); jjj--) {
                        //printf("%s << B [%d] (%d / %d) [%d]\n", __FUNCTION__, iii, jjj, src.at(iii).locations.size(), history - (src.at(iii).locations.size()-1) + jjj - 1);
                        dst.at(iii).locations.at(jjj).featureCoord = newPoints.at(currIndex);
                        //printf("%s << changed from (%f, %f) to (%f, %f)\n", __FUNCTION__, tempPoints.at(currIndex).x, tempPoints.at(currIndex).y, newPoints.at(currIndex).x, newPoints.at(currIndex).y);
                        currIndex++;
                }
                
        }
}

bool createTrackMatrix(const vector<featureTrack>& src, Mat& dst, int latest) {
        
        unsigned int rows = 240, cols = 640;
        dst = Mat(rows, cols, CV_8UC3);
        
        // printf("%s << A.\n", __FUNCTION__);
        
        if (latest == -1) {
                for (unsigned int iii = 0; iii < src.size(); iii++) {
                        
                        if (src.at(iii).locations.size() > 0) {
                                if (src.at(iii).locations.at(src.at(iii).locations.size()-1).imageIndex > latest) {
                                        latest = src.at(iii).locations.at(src.at(iii).locations.size()-1).imageIndex;
                                }
                        }
                        
                        
                }
        }
        
        // printf("%s << B.\n", __FUNCTION__);
        
        vector<int> activeTrackIndices;
        
        for (unsigned int iii = 0; iii < src.size(); iii++) {
                
                if (src.at(iii).locations.size() > 0) {
                        if (src.at(iii).locations.at(src.at(iii).locations.size()-1).imageIndex == ((int) latest)) {
                                activeTrackIndices.push_back(iii);

                        }
                }
                
                
        }

        for (unsigned int iii = 0; iii < dst.rows; iii++) {
                for (unsigned int jjj = 0; jjj < dst.cols; jjj++) {
                        dst.at<Vec3b>(iii,jjj)[0] = 255;
                        dst.at<Vec3b>(iii,jjj)[1] = 255;
                        dst.at<Vec3b>(iii,jjj)[2] = 255;
                }
        }
        
        for (unsigned int iii = 0; iii < std::min((int)rows, ((int) activeTrackIndices.size())); iii++) {
                
                for (unsigned int jjj = 0; jjj < std::min((int)cols, ((int)src.at(activeTrackIndices.at(iii)).locations.size())); jjj++) {
                        
                        unsigned int column = latest - src.at(activeTrackIndices.at(iii)).locations.at(jjj).imageIndex;
                        
                        if (column < cols) {
                                dst.at<Vec3b>(iii,column)[1] = 0;
                                dst.at<Vec3b>(iii,column)[2] = 0;                               
                        }

                }
                
        }
        
        return true;
        
        
}

void assignHistoricalPoints(const vector<featureTrack>& src, unsigned int idx_1, unsigned int idx_2, vector<Point2f>& dst) {
        
        for (unsigned int iii = 0; iii < src.size(); iii++) {
                
                bool oldFound = false;
                
                for (unsigned int jjj = 0; jjj < src.at(iii).locations.size(); jjj++) {
                        if (src.at(iii).locations.at(jjj).imageIndex == idx_1) {
                                if (src.at(iii).locations.at(src.at(iii).locations.size()-1).imageIndex != idx_2) {
                                        dst.push_back(src.at(iii).locations.at(jjj).featureCoord);
                                }
                        }
                }
        }
        
}

---

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