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 
32 #include <opengv/sac_problems/relative_pose/TranslationOnlySacProblem.hpp>
33 #include <opengv/relative_pose/methods.hpp>
34 #include <opengv/triangulation/methods.hpp>
35 
36 bool
37 opengv::sac_problems::
computeModelCoefficients(const std::vector<int> & indices,model_t & outModel) const38     relative_pose::TranslationOnlySacProblem::computeModelCoefficients(
39     const std::vector<int> &indices,
40     model_t & outModel) const
41 {
42   outModel.block<3,3>(0,0) = _adapter.getR12();
43   outModel.col(3) =
44       opengv::relative_pose::twopt(_adapter,true,indices);
45 
46   return true;
47 }
48 
49 void
50 opengv::sac_problems::
getSelectedDistancesToModel(const model_t & model,const std::vector<int> & indices,std::vector<double> & scores) const51     relative_pose::TranslationOnlySacProblem::getSelectedDistancesToModel(
52     const model_t & model,
53     const std::vector<int> & indices,
54     std::vector<double> & scores) const
55 {
56   translation_t translation = model.col(3);
57   rotation_t rotation = model.block<3,3>(0,0);
58   _adapter.sett12(translation);
59   _adapter.setR12(rotation);
60 
61   model_t inverseSolution;
62   inverseSolution.block<3,3>(0,0) = rotation.transpose();
63   inverseSolution.col(3) = -inverseSolution.block<3,3>(0,0)*translation;
64 
65   Eigen::Matrix<double,4,1> p_hom;
66   p_hom[3] = 1.0;
67 
68   for( size_t i = 0; i < indices.size(); i++ )
69   {
70     p_hom.block<3,1>(0,0) =
71         opengv::triangulation::triangulate2(_adapter,indices[i]);
72     bearingVector_t reprojection1 = p_hom.block<3,1>(0,0);
73     bearingVector_t reprojection2 = inverseSolution * p_hom;
74     reprojection1 = reprojection1 / reprojection1.norm();
75     reprojection2 = reprojection2 / reprojection2.norm();
76     bearingVector_t f1 = _adapter.getBearingVector1(indices[i]);
77     bearingVector_t f2 = _adapter.getBearingVector2(indices[i]);
78 
79     //bearing-vector based outlier criterium (select threshold accordingly):
80     //1-(f1'*f2) = 1-cos(alpha) \in [0:2]
81     double reprojError1 = 1.0 - (f1.transpose() * reprojection1);
82     double reprojError2 = 1.0 - (f2.transpose() * reprojection2);
83     scores.push_back(reprojError1 + reprojError2);
84   }
85 }
86 
87 void
88 opengv::sac_problems::
optimizeModelCoefficients(const std::vector<int> & inliers,const model_t & model,model_t & optimized_model)89     relative_pose::TranslationOnlySacProblem::optimizeModelCoefficients(
90     const std::vector<int> & inliers,
91     const model_t & model,
92     model_t & optimized_model)
93 {
94   translation_t translation = model.col(3);
95   rotation_t rotation = model.block<3,3>(0,0);
96   _adapter.sett12(translation);
97   _adapter.setR12(rotation);
98   optimized_model =
99       opengv::relative_pose::optimize_nonlinear(_adapter,inliers);
100 }
101 
102 int
103 opengv::sac_problems::
getSampleSize() const104     relative_pose::TranslationOnlySacProblem::getSampleSize() const
105 {
106   int sampleSize = 2;
107   return sampleSize;
108 }
109