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/point_cloud/methods.hpp>
38 #include <opengv/point_cloud/PointCloudAdapter.hpp>
39 #include <opengv/sac/Ransac.hpp>
40 #include <opengv/sac/Lmeds.hpp>
41 #include <opengv/sac_problems/point_cloud/PointCloudSacProblem.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.05;
61 double outlierFraction = 0.1;
62 size_t numberPoints = 100;
63
64 //generate a random pose for viewpoint 1
65 translation_t position1 = Eigen::Vector3d::Zero();
66 rotation_t rotation1 = Eigen::Matrix3d::Identity();
67
68 //generate a random pose for viewpoint 2
69 translation_t position2 = generateRandomTranslation(2.0);
70 rotation_t rotation2 = generateRandomRotation(0.5);
71
72 //derive the correspondences based on random point-cloud
73 points_t points1;
74 points_t points2;
75 Eigen::MatrixXd gt(3,numberPoints);
76 generateRandom3D3DCorrespondences(
77 position1, rotation1, position2, rotation2,
78 numberPoints, noise, outlierFraction, points1, points2, gt );
79
80 //Extract the relative pose
81 translation_t position; rotation_t rotation;
82 extractRelativePose(
83 position1, position2, rotation1, rotation2, position, rotation, false );
84
85 //print experiment characteristics
86 printExperimentCharacteristics( position, rotation, noise, outlierFraction );
87
88 //create the point-cloud adapter
89 point_cloud::PointCloudAdapter adapter(
90 points1, points2, position, rotation);
91
92 //Create a PointCloudSacProblem and Ransac
93 sac::Ransac<
94 sac_problems::point_cloud::PointCloudSacProblem> ransac;
95 std::shared_ptr<
96 sac_problems::point_cloud::PointCloudSacProblem> relposeproblem_ptr(
97 new sac_problems::point_cloud::PointCloudSacProblem(adapter));
98 ransac.sac_model_ = relposeproblem_ptr;
99 ransac.threshold_ = 0.1;
100 ransac.max_iterations_ = 50;
101
102 //Run the experiment
103 struct timeval tic;
104 struct timeval toc;
105 gettimeofday( &tic, 0 );
106 ransac.computeModel(0);
107 gettimeofday( &toc, 0 );
108 double ransac_time = TIMETODOUBLE(timeval_minus(toc,tic));
109
110 //print the results
111 std::cout << "the ransac threshold is: " << ransac.threshold_ << std::endl;
112 std::cout << "the ransac results is: " << std::endl;
113 std::cout << ransac.model_coefficients_ << std::endl << std::endl;
114 std::cout << "Ransac needed " << ransac.iterations_ << " iterations and ";
115 std::cout << ransac_time << " seconds" << std::endl << std::endl;
116 std::cout << "the number of inliers is: " << ransac.inliers_.size();
117 std::cout << std::endl << std::endl;
118 std::cout << "the found inliers are: " << std::endl;
119 for(size_t i = 0; i < ransac.inliers_.size(); i++)
120 std::cout << ransac.inliers_[i] << " ";
121 std::cout << std::endl << std::endl;
122
123 // Create Lmeds
124 sac::Lmeds<
125 sac_problems::point_cloud::PointCloudSacProblem> lmeds;
126 lmeds.sac_model_ = relposeproblem_ptr;
127 lmeds.threshold_ = 0.1;
128 lmeds.max_iterations_ = 50;
129
130 //Run the experiment
131 gettimeofday( &tic, 0 );
132 lmeds.computeModel(0);
133 gettimeofday( &toc, 0 );
134 double lmeds_time = TIMETODOUBLE(timeval_minus(toc,tic));
135
136 //print the results
137 std::cout << "the lmeds threshold is: " << lmeds.threshold_ << std::endl;
138 std::cout << "the lmeds results is: " << std::endl;
139 std::cout << lmeds.model_coefficients_ << std::endl << std::endl;
140 std::cout << "Lmeds needed " << lmeds.iterations_ << " iterations and ";
141 std::cout << lmeds_time << " seconds" << std::endl << std::endl;
142 std::cout << "the number of inliers is: " << lmeds.inliers_.size();
143 std::cout << std::endl << std::endl;
144 std::cout << "the found inliers are: " << std::endl;
145 for(size_t i = 0; i < lmeds.inliers_.size(); i++)
146 std::cout << lmeds.inliers_[i] << " ";
147 std::cout << std::endl << std::endl;
148 }
149