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