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/point_cloud/PointCloudSacProblem.hpp> 33 #include <opengv/point_cloud/methods.hpp> 34 35 bool 36 opengv::sac_problems:: computeModelCoefficients(const std::vector<int> & indices,model_t & outModel) const37 point_cloud::PointCloudSacProblem::computeModelCoefficients( 38 const std::vector<int> &indices, 39 model_t & outModel) const 40 { 41 outModel = opengv::point_cloud::threept_arun(_adapter,indices); 42 return true; 43 } 44 45 void 46 opengv::sac_problems:: getSelectedDistancesToModel(const model_t & model,const std::vector<int> & indices,std::vector<double> & scores) const47 point_cloud::PointCloudSacProblem::getSelectedDistancesToModel( 48 const model_t & model, 49 const std::vector<int> & indices, 50 std::vector<double> & scores) const 51 { 52 Eigen::Matrix<double,4,1> p_hom; 53 p_hom[3] = 1.0; 54 55 for( size_t i = 0; i < indices.size(); i++ ) 56 { 57 p_hom.block<3,1>(0,0) = _adapter.getPoint2(indices[i]); 58 point_t transformedPoint = model * p_hom; 59 translation_t error = _adapter.getPoint1(indices[i]) - transformedPoint; 60 scores.push_back(error.norm()); 61 } 62 } 63 64 void 65 opengv::sac_problems:: optimizeModelCoefficients(const std::vector<int> & inliers,const model_t & model,model_t & optimized_model)66 point_cloud::PointCloudSacProblem::optimizeModelCoefficients( 67 const std::vector<int> & inliers, 68 const model_t & model, 69 model_t & optimized_model) 70 { 71 optimized_model = opengv::point_cloud::threept_arun(_adapter,inliers); 72 } 73 74 int 75 opengv::sac_problems:: getSampleSize() const76 point_cloud::PointCloudSacProblem::getSampleSize() const 77 { 78 int sampleSize = 3; 79 return sampleSize; 80 } 81