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 "random_generators.hpp"
32 #include "time_measurement.hpp"
33 #include <math.h>
34 #include <iostream>
35 
36 
37 using namespace Eigen;
38 
39 void
initializeRandomSeed()40 opengv::initializeRandomSeed()
41 {
42   struct timeval tic;
43   gettimeofday( &tic, 0 );
44   srand ( tic.tv_usec );
45 }
46 
47 Eigen::Vector3d
generateRandomPoint(double maximumDepth,double minimumDepth)48 opengv::generateRandomPoint( double maximumDepth, double minimumDepth )
49 {
50   Eigen::Vector3d cleanPoint;
51   cleanPoint[0] = (((double) rand())/ ((double) RAND_MAX)-0.5)*2.0;
52   cleanPoint[1] = (((double) rand())/ ((double) RAND_MAX)-0.5)*2.0;
53   cleanPoint[2] = (((double) rand())/ ((double) RAND_MAX)-0.5)*2.0;
54   Eigen::Vector3d direction = cleanPoint / cleanPoint.norm();
55   cleanPoint =
56       (maximumDepth-minimumDepth) * cleanPoint + minimumDepth * direction;
57   return cleanPoint;
58 }
59 
60 Eigen::Vector3d
generateRandomPointPlane()61 opengv::generateRandomPointPlane()
62 {
63   Eigen::Vector3d cleanPoint;
64   cleanPoint[0] = (((double) rand())/ ((double) RAND_MAX)-0.5)*2.0;
65   cleanPoint[1] = (((double) rand())/ ((double) RAND_MAX)-0.5)*2.0;
66   cleanPoint[2] = (((double) rand())/ ((double) RAND_MAX)-0.5)*2.0;
67 
68   cleanPoint[0] = 6*cleanPoint[0];
69   cleanPoint[1] = 6*cleanPoint[1];
70   cleanPoint[2] = 2*cleanPoint[2]-6.0;
71 
72   return cleanPoint;
73 }
74 
75 Eigen::Vector3d
addNoise(double noiseLevel,Eigen::Vector3d cleanPoint)76 opengv::addNoise( double noiseLevel, Eigen::Vector3d cleanPoint )
77 {
78   //compute a vector in the normal plane (based on good conditioning)
79   Eigen::Vector3d normalVector1;
80   if(
81       (fabs(cleanPoint[0]) > fabs(cleanPoint[1])) &&
82       (fabs(cleanPoint[0]) > fabs(cleanPoint[2])) )
83   {
84     normalVector1[1] = 1.0;
85     normalVector1[2] = 0.0;
86     normalVector1[0] = -cleanPoint[1]/cleanPoint[0];
87   }
88   else
89   {
90     if(
91         (fabs(cleanPoint[1]) > fabs(cleanPoint[0])) &&
92         (fabs(cleanPoint[1]) > fabs(cleanPoint[2])) )
93     {
94       normalVector1[2] = 1.0;
95       normalVector1[0] = 0.0;
96       normalVector1[1] = -cleanPoint[2]/cleanPoint[1];
97     }
98     else
99     {
100       normalVector1[0] = 1.0;
101       normalVector1[1] = 0.0;
102       normalVector1[2] = -cleanPoint[0]/cleanPoint[2];
103     }
104   }
105 
106   normalVector1 = normalVector1 / normalVector1.norm();
107   Eigen::Vector3d normalVector2 = cleanPoint.cross(normalVector1);
108   double noiseX =
109       noiseLevel * (((double) rand())/ ((double) RAND_MAX)-0.5)*2.0 / 1.4142;
110   double noiseY =
111       noiseLevel * (((double) rand())/ ((double) RAND_MAX)-0.5)*2.0 / 1.4142;
112 
113   Eigen::Vector3d noisyPoint =
114       800 * cleanPoint + noiseX *normalVector1 + noiseY * normalVector2;
115   noisyPoint = noisyPoint / noisyPoint.norm();
116   return noisyPoint;
117 
118 }
119 
120 Eigen::Vector3d
generateRandomTranslation(double maximumParallax)121 opengv::generateRandomTranslation( double maximumParallax )
122 {
123   Eigen::Vector3d translation;
124   translation[0] = (((double) rand())/ ((double) RAND_MAX)-0.5)*2.0;
125   translation[1] = (((double) rand())/ ((double) RAND_MAX)-0.5)*2.0;
126   translation[2] = (((double) rand())/ ((double) RAND_MAX)-0.5)*2.0;
127   return maximumParallax * translation;
128 }
129 
130 Eigen::Vector3d
generateRandomDirectionTranslation(double parallax)131 opengv::generateRandomDirectionTranslation( double parallax )
132 {
133   Eigen::Matrix3d rotation = generateRandomRotation();
134   Eigen::Vector3d translation;
135   translation << 1.0, 0.0, 0.0;
136   translation = rotation * translation;
137   translation = parallax * translation;
138   return translation;
139 }
140 
141 Eigen::Matrix3d
generateRandomRotation(double maxAngle)142 opengv::generateRandomRotation( double maxAngle )
143 {
144   Eigen::Vector3d rpy;
145   rpy[0] = ((double) rand())/ ((double) RAND_MAX);
146   rpy[1] = ((double) rand())/ ((double) RAND_MAX);
147   rpy[2] = ((double) rand())/ ((double) RAND_MAX);
148 
149   rpy[0] = maxAngle*2.0*(rpy[0]-0.5);
150   rpy[1] = maxAngle*2.0*(rpy[1]-0.5);
151   rpy[2] = maxAngle*2.0*(rpy[2]-0.5);
152 
153   Eigen::Matrix3d R1;
154   R1(0,0) = 1.0;
155   R1(0,1) = 0.0;
156   R1(0,2) = 0.0;
157   R1(1,0) = 0.0;
158   R1(1,1) = cos(rpy[0]);
159   R1(1,2) = -sin(rpy[0]);
160   R1(2,0) = 0.0;
161   R1(2,1) = -R1(1,2);
162   R1(2,2) = R1(1,1);
163 
164   Eigen::Matrix3d R2;
165   R2(0,0) = cos(rpy[1]);
166   R2(0,1) = 0.0;
167   R2(0,2) = sin(rpy[1]);
168   R2(1,0) = 0.0;
169   R2(1,1) = 1.0;
170   R2(1,2) = 0.0;
171   R2(2,0) = -R2(0,2);
172   R2(2,1) = 0.0;
173   R2(2,2) = R2(0,0);
174 
175   Eigen::Matrix3d R3;
176   R3(0,0) = cos(rpy[2]);
177   R3(0,1) = -sin(rpy[2]);
178   R3(0,2) = 0.0;
179   R3(1,0) =-R3(0,1);
180   R3(1,1) = R3(0,0);
181   R3(1,2) = 0.0;
182   R3(2,0) = 0.0;
183   R3(2,1) = 0.0;
184   R3(2,2) = 1.0;
185 
186   Eigen::Matrix3d rotation = R3 * R2 * R1;
187 
188   rotation.col(0) = rotation.col(0) / rotation.col(0).norm();
189   rotation.col(2) = rotation.col(0).cross(rotation.col(1));
190   rotation.col(2) = rotation.col(2) / rotation.col(2).norm();
191   rotation.col(1) = rotation.col(2).cross(rotation.col(0));
192   rotation.col(1) = rotation.col(1) / rotation.col(1).norm();
193 
194   return rotation;
195 }
196 
197 Eigen::Matrix3d
generateRandomRotation()198 opengv::generateRandomRotation()
199 {
200   Eigen::Vector3d rpy;
201   rpy[0] = ((double) rand())/ ((double) RAND_MAX);
202   rpy[1] = ((double) rand())/ ((double) RAND_MAX);
203   rpy[2] = ((double) rand())/ ((double) RAND_MAX);
204 
205   rpy[0] = 2*M_PI*(rpy[0]-0.5);
206   rpy[1] = M_PI*(rpy[1]-0.5);
207   rpy[2] = 2*M_PI*(rpy[2]-0.5);
208 
209   Eigen::Matrix3d R1;
210   R1(0,0) = 1.0;
211   R1(0,1) = 0.0;
212   R1(0,2) = 0.0;
213   R1(1,0) = 0.0;
214   R1(1,1) = cos(rpy[0]);
215   R1(1,2) = -sin(rpy[0]);
216   R1(2,0) = 0.0;
217   R1(2,1) = -R1(1,2);
218   R1(2,2) = R1(1,1);
219 
220   Eigen::Matrix3d R2;
221   R2(0,0) = cos(rpy[1]);
222   R2(0,1) = 0.0;
223   R2(0,2) = sin(rpy[1]);
224   R2(1,0) = 0.0;
225   R2(1,1) = 1.0;
226   R2(1,2) = 0.0;
227   R2(2,0) = -R2(0,2);
228   R2(2,1) = 0.0;
229   R2(2,2) = R2(0,0);
230 
231   Eigen::Matrix3d R3;
232   R3(0,0) = cos(rpy[2]);
233   R3(0,1) = -sin(rpy[2]);
234   R3(0,2) = 0.0;
235   R3(1,0) =-R3(0,1);
236   R3(1,1) = R3(0,0);
237   R3(1,2) = 0.0;
238   R3(2,0) = 0.0;
239   R3(2,1) = 0.0;
240   R3(2,2) = 1.0;
241 
242   Eigen::Matrix3d rotation = R3 * R2 * R1;
243 
244   rotation.col(0) = rotation.col(0) / rotation.col(0).norm();
245   rotation.col(2) = rotation.col(0).cross(rotation.col(1));
246   rotation.col(2) = rotation.col(2) / rotation.col(2).norm();
247   rotation.col(1) = rotation.col(2).cross(rotation.col(0));
248   rotation.col(1) = rotation.col(1) / rotation.col(1).norm();
249 
250   return rotation;
251 }
252