1 /******************************************************************************
2 * Author: Laurent Kneip *
3 * Contact: kneip.laurent@gmail.com *
4 * License: Copyright (c) 2013 Laurent Kneip, ANU. All rights reserved. *
5 * *
6 * Redistribution and use in source and binary forms, with or without *
7 * modification, are permitted provided that the following conditions *
8 * are met: *
9 * * Redistributions of source code must retain the above copyright *
10 * notice, this list of conditions and the following disclaimer. *
11 * * Redistributions in binary form must reproduce the above copyright *
12 * notice, this list of conditions and the following disclaimer in the *
13 * documentation and/or other materials provided with the distribution. *
14 * * Neither the name of ANU nor the names of its contributors may be *
15 * used to endorse or promote products derived from this software without *
16 * specific prior written permission. *
17 * *
18 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"*
19 * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE *
20 * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE *
21 * ARE DISCLAIMED. IN NO EVENT SHALL ANU OR THE CONTRIBUTORS BE LIABLE *
22 * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL *
23 * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR *
24 * SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER *
25 * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT *
26 * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY *
27 * OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF *
28 * SUCH DAMAGE. *
29 ******************************************************************************/
30
31 #include <stdlib.h>
32 #include <stdio.h>
33 #include <iostream>
34 #include <iomanip>
35 #include <opengv/relative_pose/methods.hpp>
36 #include <opengv/relative_pose/CentralRelativeAdapter.hpp>
37 #include <opengv/sac/Ransac.hpp>
38 #include <opengv/sac/Lmeds.hpp>
39 #include <opengv/sac_problems/relative_pose/EigensolverSacProblem.hpp>
40 #include <sstream>
41 #include <fstream>
42
43 #include "random_generators.hpp"
44 #include "experiment_helpers.hpp"
45 #include "time_measurement.hpp"
46
47
48 using namespace std;
49 using namespace Eigen;
50 using namespace opengv;
51
main(int argc,char ** argv)52 int main( int argc, char** argv )
53 {
54 // initialize random seed
55 initializeRandomSeed();
56
57 //set experiment parameters
58 double noise = 0.5;
59 double outlierFraction = 0.1;
60 size_t numberPoints = 100;
61
62 //generate a random pose for viewpoint 1
63 translation_t position1 = Eigen::Vector3d::Zero();
64 rotation_t rotation1 = Eigen::Matrix3d::Identity();
65
66 //generate a random pose for viewpoint 2
67 translation_t position2 = generateRandomDirectionTranslation(0.1);
68 rotation_t rotation2 = generateRandomRotation(0.5);
69
70 //create a fake central camera
71 translations_t camOffsets;
72 rotations_t camRotations;
73 generateCentralCameraSystem( camOffsets, camRotations );
74
75 //derive correspondences based on random point-cloud
76 bearingVectors_t bearingVectors1;
77 bearingVectors_t bearingVectors2;
78 std::vector<int> camCorrespondences1; //unused in the central case
79 std::vector<int> camCorrespondences2; //unused in the central case
80 Eigen::MatrixXd gt(3,numberPoints);
81 generateRandom2D2DCorrespondences(
82 position1, rotation1, position2, rotation2,
83 camOffsets, camRotations, numberPoints, noise, outlierFraction,
84 bearingVectors1, bearingVectors2,
85 camCorrespondences1, camCorrespondences2, gt );
86
87 //Extract the relative pose
88 translation_t position; rotation_t rotation;
89 extractRelativePose(
90 position1, position2, rotation1, rotation2, position, rotation );
91
92 //print experiment characteristics
93 printExperimentCharacteristics( position, rotation, noise, outlierFraction );
94
95 //create a central relative adapter
96 relative_pose::CentralRelativeAdapter adapter(
97 bearingVectors1,
98 bearingVectors2,
99 rotation);
100
101 //Create an EigensolverSacProblem and Ransac
102 //The number of samples can be configured
103 sac::Ransac<
104 sac_problems::relative_pose::EigensolverSacProblem> ransac;
105 std::shared_ptr<
106 sac_problems::relative_pose::EigensolverSacProblem> eigenproblem_ptr(
107 new sac_problems::relative_pose::EigensolverSacProblem(adapter,10));
108 ransac.sac_model_ = eigenproblem_ptr;
109 ransac.threshold_ = 1.0;
110 ransac.max_iterations_ = 100;
111
112 //Run the experiment
113 struct timeval tic;
114 struct timeval toc;
115 gettimeofday( &tic, 0 );
116 ransac.computeModel();
117 gettimeofday( &toc, 0 );
118 double ransac_time = TIMETODOUBLE(timeval_minus(toc,tic));
119
120 //do final polishing of the model over all inliers
121 sac_problems::relative_pose::EigensolverSacProblem::model_t optimizedModel;
122 eigenproblem_ptr->optimizeModelCoefficients(
123 ransac.inliers_,
124 ransac.model_coefficients_,
125 optimizedModel);
126
127 //print the results
128 std::cout << "the ransac results is: " << std::endl;
129 std::cout << ransac.model_coefficients_.rotation << std::endl << std::endl;
130 std::cout << "Ransac needed " << ransac.iterations_ << " iterations and ";
131 std::cout << ransac_time << " seconds" << std::endl << std::endl;
132 std::cout << "the number of inliers is: " << ransac.inliers_.size();
133 std::cout << std::endl << std::endl;
134 std::cout << "the found inliers are: " << std::endl;
135 for(size_t i = 0; i < ransac.inliers_.size(); i++)
136 std::cout << ransac.inliers_[i] << " ";
137 std::cout << std::endl << std::endl;
138 std::cout << "the optimized result is: " << std::endl;
139 std::cout << optimizedModel.rotation << std::endl;
140
141 // Create Lmeds
142 sac::Lmeds<sac_problems::relative_pose::EigensolverSacProblem> lmeds;
143 lmeds.sac_model_ = eigenproblem_ptr;
144 lmeds.threshold_ = 1.0;
145 lmeds.max_iterations_ = 50;
146
147 //Run the experiment
148 gettimeofday( &tic, 0 );
149 lmeds.computeModel();
150 gettimeofday( &toc, 0 );
151 double lmeds_time = TIMETODOUBLE(timeval_minus(toc,tic));
152
153 //print the results
154 std::cout << "the lmeds results is: " << std::endl;
155 std::cout << lmeds.model_coefficients_.rotation << std::endl << std::endl;
156 std::cout << "lmeds needed " << lmeds.iterations_ << " iterations and ";
157 std::cout << lmeds_time << " seconds" << std::endl << std::endl;
158 std::cout << "the number of inliers is: " << lmeds.inliers_.size();
159 std::cout << std::endl << std::endl;
160 std::cout << "the found inliers are: " << std::endl;
161 for(size_t i = 0; i < lmeds.inliers_.size(); i++)
162 std::cout << lmeds.inliers_[i] << " ";
163 std::cout << std::endl << std::endl;
164 }
165