summaryrefslogtreecommitdiff
diff options
context:
space:
mode:
authorGuillaume Seguin <guillaume@segu.in>2010-06-25 14:26:43 -0400
committerGuillaume Seguin <guillaume@segu.in>2010-06-25 14:26:43 -0400
commitc43fb7cb98bc36cf0b97652492e748a191ef7939 (patch)
tree393461e37d837df5def987a04646b5d6e4d2e33a
parent2855df5c4610c4414258dd53de3e2cd21fde623f (diff)
downloadcmu-c43fb7cb98bc36cf0b97652492e748a191ef7939.tar.gz
cmu-c43fb7cb98bc36cf0b97652492e748a191ef7939.tar.bz2
Woops, missing file
-rw-r--r--libmoped/src/sfm/SFM_T.hpp224
1 files changed, 224 insertions, 0 deletions
diff --git a/libmoped/src/sfm/SFM_T.hpp b/libmoped/src/sfm/SFM_T.hpp
new file mode 100644
index 0000000..b551f68
--- /dev/null
+++ b/libmoped/src/sfm/SFM_T.hpp
@@ -0,0 +1,224 @@
+#pragma once
+
+using namespace std;
+using namespace cv;
+
+namespace MopedNS {
+
+ TrifocalTensor findTrifocalTensor (vector<Point2f> points1,
+ vector<Point2f> points2,
+ vector<Point2f> points3)
+ {
+ /* FIXME : add some checks on the input */
+ int points_count = points1.size ();
+ Mat A (4 * points_count, 27, CV_64FC1);
+
+ for (int i = 0; i < points_count; i++)
+ {
+ int i4 = 4 * i;
+ double u0 = points1[i].x;
+ double v0 = points1[i].y;
+ double u1 = points2[i].x;
+ double v1 = points2[i].y;
+ double u2 = points3[i].x;
+ double v2 = points3[i].y;
+ A.at<double> (i4 , 1-1) = u0;
+ A.at<double> (i4 , 2-1) = 0;
+ A.at<double> (i4 , 3-1) = - u0 * u2;
+ A.at<double> (i4 , 4-1) = 0;
+ A.at<double> (i4 , 5-1) = 0;
+ A.at<double> (i4 , 6-1) = 0;
+ A.at<double> (i4 , 7-1) = - u0 * u1;
+ A.at<double> (i4 , 8-1) = 0;
+ A.at<double> (i4 , 9-1) = u0 * u1 * u2;
+ A.at<double> (i4 , 10-1) = v0;
+ A.at<double> (i4 , 11-1) = 0;
+ A.at<double> (i4 , 12-1) = - u2 * v0;
+ A.at<double> (i4 , 13-1) = 0;
+ A.at<double> (i4 , 14-1) = 0;
+ A.at<double> (i4 , 15-1) = 0;
+ A.at<double> (i4 , 16-1) = - u1 * v0;
+ A.at<double> (i4 , 17-1) = 0;
+ A.at<double> (i4 , 18-1) = u1 * u2 * v0;
+ A.at<double> (i4 , 19-1) = 1;
+ A.at<double> (i4 , 20-1) = 0;
+ A.at<double> (i4 , 21-1) = - u2;
+ A.at<double> (i4 , 22-1) = 0;
+ A.at<double> (i4 , 23-1) = 0;
+ A.at<double> (i4 , 24-1) = 0;
+ A.at<double> (i4 , 25-1) = - u1;
+ A.at<double> (i4 , 26-1) = 0;
+ A.at<double> (i4 , 27-1) = u1 * u2;
+ A.at<double> (i4 + 1, 1-1) = 0;
+ A.at<double> (i4 + 1, 2-1) = u0;
+ A.at<double> (i4 + 1, 3-1) = - u0 * v2;
+ A.at<double> (i4 + 1, 4-1) = 0;
+ A.at<double> (i4 + 1, 5-1) = 0;
+ A.at<double> (i4 + 1, 6-1) = 0;
+ A.at<double> (i4 + 1, 7-1) = 0;
+ A.at<double> (i4 + 1, 8-1) = - u0 * u1;
+ A.at<double> (i4 + 1, 9-1) = u0 * u1 * v2;
+ A.at<double> (i4 + 1, 10-1) = 0;
+ A.at<double> (i4 + 1, 11-1) = v0;
+ A.at<double> (i4 + 1, 12-1) = - v0 * v2;
+ A.at<double> (i4 + 1, 13-1) = 0;
+ A.at<double> (i4 + 1, 14-1) = 0;
+ A.at<double> (i4 + 1, 15-1) = 0;
+ A.at<double> (i4 + 1, 16-1) = 0;
+ A.at<double> (i4 + 1, 17-1) = - u1 * v0;
+ A.at<double> (i4 + 1, 18-1) = u1 * v0 * v2;
+ A.at<double> (i4 + 1, 19-1) = 0;
+ A.at<double> (i4 + 1, 20-1) = 1;
+ A.at<double> (i4 + 1, 21-1) = - v2;
+ A.at<double> (i4 + 1, 22-1) = 0;
+ A.at<double> (i4 + 1, 23-1) = 0;
+ A.at<double> (i4 + 1, 24-1) = 0;
+ A.at<double> (i4 + 1, 25-1) = 0;
+ A.at<double> (i4 + 1, 26-1) = - u1;
+ A.at<double> (i4 + 1, 27-1) = u1 * v2;
+ A.at<double> (i4 + 2, 1-1) = 0;
+ A.at<double> (i4 + 2, 2-1) = 0;
+ A.at<double> (i4 + 2, 3-1) = 0;
+ A.at<double> (i4 + 2, 4-1) = u0;
+ A.at<double> (i4 + 2, 5-1) = 0;
+ A.at<double> (i4 + 2, 6-1) = - u0 * u2;
+ A.at<double> (i4 + 2, 7-1) = - u0 * v1;
+ A.at<double> (i4 + 2, 8-1) = 0;
+ A.at<double> (i4 + 2, 9-1) = u0 * u2 * v1;
+ A.at<double> (i4 + 2, 10-1) = 0;
+ A.at<double> (i4 + 2, 11-1) = 0;
+ A.at<double> (i4 + 2, 12-1) = 0;
+ A.at<double> (i4 + 2, 13-1) = v0;
+ A.at<double> (i4 + 2, 14-1) = 0;
+ A.at<double> (i4 + 2, 15-1) = - u2 * v0;
+ A.at<double> (i4 + 2, 16-1) = - v0 * v1;
+ A.at<double> (i4 + 2, 17-1) = 0;
+ A.at<double> (i4 + 2, 18-1) = u2 * v0 * v1;
+ A.at<double> (i4 + 2, 19-1) = 0;
+ A.at<double> (i4 + 2, 20-1) = 0;
+ A.at<double> (i4 + 2, 21-1) = 0;
+ A.at<double> (i4 + 2, 22-1) = 1;
+ A.at<double> (i4 + 2, 23-1) = 0;
+ A.at<double> (i4 + 2, 24-1) = - u2;
+ A.at<double> (i4 + 2, 25-1) = - v1;
+ A.at<double> (i4 + 2, 26-1) = 0;
+ A.at<double> (i4 + 2, 27-1) = u2 * v1;
+ A.at<double> (i4 + 3, 1-1) = 0;
+ A.at<double> (i4 + 3, 2-1) = 0;
+ A.at<double> (i4 + 3, 3-1) = 0;
+ A.at<double> (i4 + 3, 4-1) = 0;
+ A.at<double> (i4 + 3, 5-1) = u0;
+ A.at<double> (i4 + 3, 6-1) = - u0 * v2;
+ A.at<double> (i4 + 3, 7-1) = 0;
+ A.at<double> (i4 + 3, 8-1) = - u0 * v1;
+ A.at<double> (i4 + 3, 9-1) = u0 * v1 * v2;
+ A.at<double> (i4 + 3, 10-1) = 0;
+ A.at<double> (i4 + 3, 11-1) = 0;
+ A.at<double> (i4 + 3, 12-1) = 0;
+ A.at<double> (i4 + 3, 13-1) = 0;
+ A.at<double> (i4 + 3, 14-1) = v0;
+ A.at<double> (i4 + 3, 15-1) = - v2 * v0;
+ A.at<double> (i4 + 3, 16-1) = 0;
+ A.at<double> (i4 + 3, 17-1) = - v0 * v1;
+ A.at<double> (i4 + 3, 18-1) = v0 * v1 * v2;
+ A.at<double> (i4 + 3, 19-1) = 0;
+ A.at<double> (i4 + 3, 20-1) = 0;
+ A.at<double> (i4 + 3, 21-1) = 0;
+ A.at<double> (i4 + 3, 22-1) = 0;
+ A.at<double> (i4 + 3, 23-1) = 1;
+ A.at<double> (i4 + 3, 24-1) = - v2;
+ A.at<double> (i4 + 3, 25-1) = 0;
+ A.at<double> (i4 + 3, 26-1) = - v1;
+ A.at<double> (i4 + 3, 27-1) = v1 * v2;
+ }
+
+ Mat x (26, 1, CV_64FC1);
+ SVD::solveZ (A, x);
+
+ TrifocalTensor ret;
+ for (int m = 0; m < 3; ++m)
+ {
+ ret.m[m] = Mat (3, 3, CV_64FC1);
+ for (int i = 0; i < 3; ++i)
+ {
+ for (int j = 0; j < 3; ++j)
+ {
+ ret.m[m].at<double> (i, j) = x.at<double> (m * 9 + i * 3 + j, 1);
+ }
+ }
+ }
+
+ /* Now we should do some RANSAC to filter out outliers */
+
+ return ret;
+ }
+
+ class SFM_T:public MopedAlg {
+
+ public:
+
+ /* FIXME : doing copy here, looks really bad */
+ vector<Point2f> prev_new_points;
+ vector<KeyPoint> prev_key_points;
+ vector<int> prev_old_points_indices;
+
+ SFM_T()
+ {}
+
+ void getConfig (map<string,string> &config) const {
+ }
+
+ void setConfig (map<string,string> &config) {
+ }
+
+ void process (FrameData &frameData) {
+ for (int i = 0; i < (int) frameData.images.size(); i++)
+ {
+ vector<Point2f> & new_points = frameData.trackingNewPoints[i];
+ vector<KeyPoint> & key_points = frameData.trackedFeatures[i];
+ vector<int> & old_points_indices = frameData.trackingPrevIndices[i];
+#ifdef PREVPREV_TRACKING
+ vector<Point2f> & prevprev_new_points = frameData.prevTrackingNewPoints[i];
+ vector<KeyPoint> & prevprev_key_points = frameData.prevTrackedFeatures[i];
+ vector<int> & prevprev_old_points_indices = frameData.prevTrackingPrevIndices[i];
+#endif
+ int points_count = prevprev_old_points_indices.size ();
+ int prev_points_count = prev_old_points_indices.size ();
+ if (points_count == 0)
+ return;
+ if (prev_points_count == 0)
+ {
+ prev_new_points = new_points;
+ prev_key_points = key_points;
+ prev_old_points_indices = old_points_indices;
+ return;
+ }
+ vector<Point2f> points1 (points_count);
+ vector<Point2f> points2 (points_count);
+ vector<Point2f> points3 (points_count);
+ int k = 0;
+ for (int j = 0; j < points_count; ++j)
+ {
+ int kp_index = prevprev_old_points_indices[j];
+ vector<int>::iterator find_it =
+ find (prev_old_points_indices.begin (),
+ prev_old_points_indices.end (),
+ kp_index);
+ if (find_it == prev_old_points_indices.end ())
+ continue;
+ int index = distance (prev_old_points_indices.begin (),
+ find_it);
+ points1[k] = prevprev_key_points[kp_index].pt;
+ points2[k] = prev_new_points[index];
+ points3[k] = prevprev_new_points[j];
+ ++k;
+ }
+ frameData.T[i] = findTrifocalTensor(points1, points2, points3);
+ /* Update prev */
+ prev_new_points = new_points;
+ prev_key_points = key_points;
+ prev_old_points_indices = old_points_indices;
+ }
+ }
+ };
+};