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 <limits.h>
36 #include <Eigen/Eigen>
37 #include <opengv/absolute_pose/methods.hpp>
38 #include <opengv/absolute_pose/CentralAbsoluteAdapter.hpp>
39 #include <opengv/sac/Ransac.hpp>
40 #include <opengv/sac/Lmeds.hpp>
41 #include <opengv/sac_problems/absolute_pose/AbsolutePoseSacProblem.hpp>
42 #include <sstream>
43 #include <fstream>
44
45 #include "random_generators.hpp"
46 #include "experiment_helpers.hpp"
47 #include "time_measurement.hpp"
48
49
50 using namespace std;
51 using namespace Eigen;
52 using namespace opengv;
53
main(int argc,char ** argv)54 int main( int argc, char** argv )
55 {
56 //initialize random seed
57 initializeRandomSeed();
58
59 //set experiment parameters
60 double noise = 0.0;
61 double outlierFraction = 0.1;
62 size_t numberPoints = 100;
63
64 //create a random viewpoint pose
65 translation_t position = generateRandomTranslation(2.0);
66 rotation_t rotation = generateRandomRotation(0.5);
67
68 //create a fake central camera
69 translations_t camOffsets;
70 rotations_t camRotations;
71 generateCentralCameraSystem( camOffsets, camRotations );
72
73 //derive correspondences based on random point-cloud
74 bearingVectors_t bearingVectors;
75 points_t points;
76 std::vector<int> camCorrespondences; //unused in the central case!
77 Eigen::MatrixXd gt(3,numberPoints);
78 generateRandom2D3DCorrespondences(
79 position, rotation, camOffsets, camRotations, numberPoints, noise, outlierFraction,
80 bearingVectors, points, camCorrespondences, gt );
81
82 //print the experiment characteristics
83 printExperimentCharacteristics(
84 position, rotation, noise, outlierFraction );
85
86 //create a central absolute adapter
87 absolute_pose::CentralAbsoluteAdapter adapter(
88 bearingVectors,
89 points,
90 rotation);
91
92 //Create an AbsolutePoseSac problem and Ransac
93 //The method can be set to KNEIP, GAO or EPNP
94 sac::Ransac<sac_problems::absolute_pose::AbsolutePoseSacProblem> ransac;
95 std::shared_ptr<
96 sac_problems::absolute_pose::AbsolutePoseSacProblem> absposeproblem_ptr(
97 new sac_problems::absolute_pose::AbsolutePoseSacProblem(
98 adapter,
99 sac_problems::absolute_pose::AbsolutePoseSacProblem::KNEIP));
100 ransac.sac_model_ = absposeproblem_ptr;
101 ransac.threshold_ = 1.0 - cos(atan(sqrt(2.0)*0.5/800.0));
102 ransac.max_iterations_ = 50;
103
104 //Run the experiment
105 struct timeval tic;
106 struct timeval toc;
107 gettimeofday( &tic, 0 );
108 ransac.computeModel();
109 gettimeofday( &toc, 0 );
110 double ransac_time = TIMETODOUBLE(timeval_minus(toc,tic));
111
112 //print the results
113 std::cout << "the ransac results is: " << std::endl;
114 std::cout << ransac.model_coefficients_ << std::endl << std::endl;
115 std::cout << "Ransac needed " << ransac.iterations_ << " iterations and ";
116 std::cout << ransac_time << " seconds" << std::endl << std::endl;
117 std::cout << "the number of inliers is: " << ransac.inliers_.size();
118 std::cout << std::endl << std::endl;
119 std::cout << "the found inliers are: " << std::endl;
120 for(size_t i = 0; i < ransac.inliers_.size(); i++)
121 std::cout << ransac.inliers_[i] << " ";
122 std::cout << std::endl << std::endl;
123
124 // Create LMedS
125 sac::Lmeds<sac_problems::absolute_pose::AbsolutePoseSacProblem> lmeds;
126 lmeds.sac_model_ = absposeproblem_ptr;
127 lmeds.threshold_ = 1.0 - cos(atan(sqrt(2.0)*0.5/800.0));
128 lmeds.max_iterations_ = 50;
129
130 //Run the LMedS experiment
131 gettimeofday( &tic, 0 );
132 lmeds.computeModel();
133 gettimeofday( &toc, 0 );
134 double lmeds_time = TIMETODOUBLE(timeval_minus(toc,tic));
135
136 //print the results
137 std::cout << "the lmeds results is: " << std::endl;
138 std::cout << lmeds.model_coefficients_ << std::endl << std::endl;
139 std::cout << "Lmeds needed " << lmeds.iterations_ << " iterations and ";
140 std::cout << lmeds_time << " seconds" << std::endl << std::endl;
141 std::cout << "the number of inliers is: " << lmeds.inliers_.size();
142 std::cout << std::endl << std::endl;
143 std::cout << "the found inliers are: " << std::endl;
144 for(size_t i = 0; i < lmeds.inliers_.size(); i++)
145 std::cout << lmeds.inliers_[i] << " ";
146 std::cout << std::endl << std::endl;
147 }
148