1 // This is oxl/mvl/FMatrixComputeLinear.cxx
2
3 //-----------------------------------------------------------------------------
4 //
5 // .NAME FMatrixComputeLinear
6 // Author: Andrew W. Fitzgibbon, Oxford RRG
7 // Created: 23 Jul 96
8 //
9 //-----------------------------------------------------------------------------
10
11 #include <vector>
12 #include <iostream>
13 #include "FMatrixComputeLinear.h"
14
15 #ifdef _MSC_VER
16 # include "vcl_msvc_warnings.h"
17 #endif
18
19 #include <vnl/algo/vnl_svd.h>
20 #include "vgl/vgl_homg_point_2d.h"
21
22 #include <mvl/HomgMetric.h>
23 #include <mvl/PairMatchSetCorner.h>
24 #include <mvl/FDesignMatrix.h>
25 #include <mvl/HomgNorm2D.h>
26
FMatrixComputeLinear(bool precondition,bool rank2_truncate)27 FMatrixComputeLinear::FMatrixComputeLinear(bool precondition, bool rank2_truncate):
28 precondition_(precondition),
29 rank2_truncate_(rank2_truncate)
30 {
31 }
32
33 //-----------------------------------------------------------------------------
34 //
35 // - Compute a fundamental matrix for a set of point matches.
36 //
37 // Return false if the calculation fails or there are fewer than eight point
38 // matches in the list.
39 //
40
compute(PairMatchSetCorner & matches,FMatrix * F)41 bool FMatrixComputeLinear::compute (PairMatchSetCorner& matches, FMatrix *F)
42 {
43 // Copy matching points from matchset.
44 std::vector<HomgPoint2D> points1(matches.count());
45 std::vector<HomgPoint2D> points2(matches.count());
46 matches.extract_matches(points1, points2);
47 return compute(points1, points2, F);
48 }
49
50 //-----------------------------------------------------------------------------
compute(std::vector<vgl_homg_point_2d<double>> & points1,std::vector<vgl_homg_point_2d<double>> & points2,FMatrix & F)51 bool FMatrixComputeLinear::compute (std::vector<vgl_homg_point_2d<double> >& points1,
52 std::vector<vgl_homg_point_2d<double> >& points2, FMatrix& F)
53 {
54 if (points1.size() < 8 || points2.size() < 8) {
55 std::cerr << "FMatrixComputeLinear: Need at least 8 point pairs.\n"
56 << "Number in each set: " << points1.size() << ", " << points2.size() << std::endl;
57 return false;
58 }
59
60 if (precondition_) {
61 // Condition points
62 HomgNorm2D conditioned1(points1);
63 HomgNorm2D conditioned2(points2);
64
65 // Compute F with preconditioned points
66 compute_preconditioned(conditioned1.get_normalized_points(),
67 conditioned2.get_normalized_points(),
68 &F);
69
70 // De-condition F
71 F = HomgMetric::homg_to_image_F(F, &conditioned1, &conditioned2);
72 } else
73 compute_preconditioned(points1, points2, F);
74
75 return true;
76 }
77
78 //-----------------------------------------------------------------------------
compute(std::vector<HomgPoint2D> & points1,std::vector<HomgPoint2D> & points2,FMatrix * F)79 bool FMatrixComputeLinear::compute (std::vector<HomgPoint2D>& points1,
80 std::vector<HomgPoint2D>& points2, FMatrix *F)
81 {
82 if (points1.size() < 8 || points2.size() < 8) {
83 std::cerr << "FMatrixComputeLinear: Need at least 8 point pairs.\n"
84 << "Number in each set: " << points1.size() << ", " << points2.size() << std::endl;
85 return false;
86 }
87
88 if (precondition_) {
89 // Condition points
90 HomgNorm2D conditioned1(points1);
91 HomgNorm2D conditioned2(points2);
92
93 // Compute F with preconditioned points
94 compute_preconditioned(conditioned1.get_normalized_points(),
95 conditioned2.get_normalized_points(),
96 F);
97
98 // De-condition F
99 *F = HomgMetric::homg_to_image_F(*F, &conditioned1, &conditioned2);
100 } else
101 compute_preconditioned(points1, points2, F);
102
103 return true;
104 }
105
106 //-----------------------------------------------------------------------------
compute_preconditioned(std::vector<vgl_homg_point_2d<double>> & points1,std::vector<vgl_homg_point_2d<double>> & points2,FMatrix & F) const107 bool FMatrixComputeLinear::compute_preconditioned (std::vector<vgl_homg_point_2d<double> >& points1,
108 std::vector<vgl_homg_point_2d<double> >& points2,
109 FMatrix& F) const
110 {
111 // Create design matrix from conditioned points.
112 FDesignMatrix design(points1, points2);
113
114 // Normalize rows for better conditioning
115 design.normalize_rows();
116
117 // Extract vnl_svd<double> of design matrix
118 vnl_svd<double> svd(design);
119
120 // Reshape nullvector to 3x3
121 F.set(vnl_double_3x3(svd.nullvector().data_block()));
122
123 // Rank-truncate F
124 if (rank2_truncate_)
125 F.set_rank2_using_svd();
126
127 return true;
128 }
129
130 //-----------------------------------------------------------------------------
compute_preconditioned(std::vector<HomgPoint2D> & points1,std::vector<HomgPoint2D> & points2,FMatrix * F) const131 bool FMatrixComputeLinear::compute_preconditioned (std::vector<HomgPoint2D>& points1,
132 std::vector<HomgPoint2D>& points2,
133 FMatrix *F) const
134 {
135 // Create design matrix from conditioned points.
136 FDesignMatrix design(points1, points2);
137
138 // Normalize rows for better conditioning
139 design.normalize_rows();
140
141 // Extract vnl_svd<double> of design matrix
142 vnl_svd<double> svd (design);
143
144 // Reshape nullvector to 3x3
145 F->set(vnl_double_3x3(svd.nullvector().data_block()));
146
147 // Rank-truncate F
148 if (rank2_truncate_)
149 F->set_rank2_using_svd();
150
151 return true;
152 }
153