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/absolute_pose/MultiNoncentralAbsolutePoseSacProblem.hpp> 33 #include <opengv/absolute_pose/methods.hpp> 34 35 bool 36 opengv::sac_problems:: computeModelCoefficients(const std::vector<std::vector<int>> & indices,model_t & outModel) const37 absolute_pose::MultiNoncentralAbsolutePoseSacProblem::computeModelCoefficients( 38 const std::vector< std::vector<int> > & indices, 39 model_t & outModel) const 40 { 41 transformations_t solutions; 42 std::vector<int> stratifiedIndices = _adapter.convertMultiIndices(indices); 43 44 if(!_asCentral) 45 { 46 solutions = opengv::absolute_pose::gp3p( 47 _adapter,stratifiedIndices); 48 } 49 else 50 { 51 solutions = opengv::absolute_pose::p3p_kneip( 52 _adapter,stratifiedIndices); 53 54 //transform solution into body frame (case of single shifted cam) 55 translation_t t_bc = _adapter.getCamOffset(0); 56 rotation_t R_bc = _adapter.getCamRotation(0); 57 58 for(size_t i = 0; i < solutions.size(); i++) 59 { 60 translation_t translation = solutions[i].col(3); 61 rotation_t rotation = solutions[i].block<3,3>(0,0); 62 solutions[i].col(3) = translation - rotation * R_bc.transpose() * t_bc; 63 solutions[i].block<3,3>(0,0) = rotation * R_bc.transpose(); 64 } 65 } 66 67 if( solutions.size() == 1 ) 68 { 69 outModel = solutions[0]; 70 return true; 71 } 72 73 //now compute reprojection error of fourth point, in order to find the right one 74 double minScore = 1000000.0; 75 int minIndex = -1; 76 for(size_t i = 0; i < solutions.size(); i++) 77 { 78 //compute inverse transformation 79 model_t inverseSolution; 80 inverseSolution.block<3,3>(0,0) = solutions[i].block<3,3>(0,0).transpose(); 81 inverseSolution.col(3) = -inverseSolution.block<3,3>(0,0)*solutions[i].col(3); 82 83 //get the fourth point in homogeneous form 84 Eigen::Matrix<double,4,1> p_hom; 85 p_hom.block<3,1>(0,0) = _adapter.getPoint(stratifiedIndices[3]); 86 p_hom[3] = 1.0; 87 88 //compute the reprojection (this is working for both central and 89 //non-central case) 90 point_t bodyReprojection = inverseSolution * p_hom; 91 point_t reprojection = 92 _adapter.getCamRotation(stratifiedIndices[3]).transpose() * 93 (bodyReprojection - _adapter.getCamOffset(stratifiedIndices[3])); 94 reprojection = reprojection / reprojection.norm(); 95 96 //compute the score 97 double score = 98 1.0 - (reprojection.transpose() * _adapter.getBearingVector(stratifiedIndices[3])); 99 100 //check for best solution 101 if( score < minScore ) 102 { 103 minScore = score; 104 minIndex = i; 105 } 106 } 107 108 if(minIndex == -1) 109 return false; 110 outModel = solutions[minIndex]; 111 112 return true; 113 } 114 115 void 116 opengv::sac_problems:: getSelectedDistancesToModel(const model_t & model,const std::vector<std::vector<int>> & indices,std::vector<std::vector<double>> & scores) const117 absolute_pose::MultiNoncentralAbsolutePoseSacProblem::getSelectedDistancesToModel( 118 const model_t & model, 119 const std::vector<std::vector<int> > & indices, 120 std::vector<std::vector<double> > & scores) const 121 { 122 //compute the reprojection error of all points 123 124 //compute inverse transformation 125 model_t inverseSolution; 126 inverseSolution.block<3,3>(0,0) = model.block<3,3>(0,0).transpose(); 127 inverseSolution.col(3) = -inverseSolution.block<3,3>(0,0)*model.col(3); 128 129 Eigen::Matrix<double,4,1> p_hom; 130 p_hom[3] = 1.0; 131 132 for(size_t f = 0; f < indices.size(); f++ ) 133 { 134 for(size_t i = 0; i < indices[f].size(); i++) 135 { 136 //get point in homogeneous form 137 p_hom.block<3,1>(0,0) = _adapter.getPoint(f,indices[f][i]); 138 139 //compute the reprojection (this is working for both central and 140 //non-central case) 141 point_t bodyReprojection = inverseSolution * p_hom; 142 point_t reprojection = 143 _adapter.getMultiCamRotation(f).transpose() * 144 (bodyReprojection - _adapter.getMultiCamOffset(f)); 145 reprojection = reprojection / reprojection.norm(); 146 147 //compute the score 148 scores[f].push_back( 149 1.0 - (reprojection.transpose() * _adapter.getBearingVector(f,indices[f][i]))); 150 } 151 } 152 } 153 154 void 155 opengv::sac_problems:: optimizeModelCoefficients(const std::vector<std::vector<int>> & inliers,const model_t & model,model_t & optimized_model)156 absolute_pose::MultiNoncentralAbsolutePoseSacProblem::optimizeModelCoefficients( 157 const std::vector< std::vector<int> > & inliers, 158 const model_t & model, 159 model_t & optimized_model) 160 { 161 _adapter.sett(model.col(3)); 162 _adapter.setR(model.block<3,3>(0,0)); 163 optimized_model = opengv::absolute_pose::optimize_nonlinear(_adapter,_adapter.convertMultiIndices(inliers)); 164 } 165 166 std::vector<int> 167 opengv::sac_problems:: getSampleSizes() const168 absolute_pose::MultiNoncentralAbsolutePoseSacProblem::getSampleSizes() const 169 { 170 std::vector<int> sampleSizes; 171 for( size_t i = 0; i < _adapter.getNumberFrames(); i++ ) 172 sampleSizes.push_back(0); 173 174 int sampleSize = 4; 175 176 //set it to a random cam index where to start 177 size_t binIndex = floor(((double) _adapter.getNumberFrames()) * 178 ((double) rand()) / ((double) RAND_MAX)); 179 for( int i = 0; i < sampleSize; i++ ) 180 { 181 sampleSizes[binIndex]++; 182 binIndex++; 183 if(binIndex >= sampleSizes.size()) 184 binIndex = 0; 185 } 186 187 return sampleSizes; 188 } 189