OKVIS ROS
 All Classes Namespaces Files Functions Variables Typedefs Enumerations Enumerator Friends Macros Pages
FrameRelativePoseSacProblem.hpp
Go to the documentation of this file.
1 /*********************************************************************************
2  * OKVIS - Open Keyframe-based Visual-Inertial SLAM
3  * Copyright (c) 2015, Autonomous Systems Lab / ETH Zurich
4  *
5  * Redistribution and use in source and binary forms, with or without
6  * modification, are permitted provided that the following conditions are met:
7  *
8  * * Redistributions of source code must retain the above copyright notice,
9  * this list of conditions and the following disclaimer.
10  * * Redistributions in binary form must reproduce the above copyright notice,
11  * this list of conditions and the following disclaimer in the documentation
12  * and/or other materials provided with the distribution.
13  * * Neither the name of Autonomous Systems Lab / ETH Zurich nor the names of
14  * its contributors may be used to endorse or promote products derived from
15  * this software without specific prior written permission.
16  *
17  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
18  * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
19  * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
20  * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
21  * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
22  * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
23  * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
24  * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
25  * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
26  * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
27  * POSSIBILITY OF SUCH DAMAGE.
28  *
29  * Created on: Mar 7, 2013
30  * Author: Stefan Leutenegger (s.leutenegger@imperial.ac.uk)
31  *********************************************************************************/
32 
39 #ifndef INCLUDE_OKVIS_OPENGV_FRAMERELATIVEPOSESACPROBLEM_HPP_
40 #define INCLUDE_OKVIS_OPENGV_FRAMERELATIVEPOSESACPROBLEM_HPP_
41 
42 #include <okvis/assert_macros.hpp>
43 #include <opengv/types.hpp>
44 #include <opengv/triangulation/methods.hpp>
45 #include <opengv/relative_pose/methods.hpp>
46 #include <opengv/sac_problems/relative_pose/CentralRelativePoseSacProblem.hpp>
48 
52 namespace opengv {
56 namespace sac_problems {
60 namespace relative_pose {
61 
68 class FrameRelativePoseSacProblem : public CentralRelativePoseSacProblem {
69  public:
70  OKVIS_DEFINE_EXCEPTION(Exception,std::runtime_error)
71 
72  typedef CentralRelativePoseSacProblem base_t;
73 
75  using base_t::adapter_t;
77  using base_t::algorithm_t;
79  using base_t::model_t;
80 
87  FrameRelativePoseSacProblem(adapter_t & adapter, algorithm_t algorithm)
88  : base_t(adapter, algorithm),
90  *static_cast<opengv::relative_pose::FrameRelativeAdapter*>(&_adapter)) {
92  Exception,
93  dynamic_cast<opengv::relative_pose::FrameRelativeAdapter*>(&_adapter),
94  "only opengv::absolute_pose::FrameRelativeAdapter supported");
95  }
96 
105  FrameRelativePoseSacProblem(adapter_t & adapter, algorithm_t algorithm,
106  const std::vector<int> & indices)
107  : base_t(adapter, algorithm, indices),
109  *static_cast<opengv::relative_pose::FrameRelativeAdapter*>(&_adapter)) {
111  Exception,
112  dynamic_cast<opengv::relative_pose::FrameRelativeAdapter*>(&_adapter),
113  "only opengv::absolute_pose::FrameRelativeAdapter supported");
114  }
116  }
117 
126  virtual void getSelectedDistancesToModel(const model_t & model,
127  const std::vector<int> & indices,
128  std::vector<double> & scores) const {
129  translation_t translation = model.col(3);
130  rotation_t rotation = model.block<3, 3>(0, 0);
131  adapterDerived_.sett12(translation);
132  adapterDerived_.setR12(rotation);
133 
134  model_t inverseSolution;
135  inverseSolution.block<3, 3>(0, 0) = rotation.transpose();
136  inverseSolution.col(3) = -inverseSolution.block<3, 3>(0, 0) * translation;
137 
138  Eigen::Matrix<double, 4, 1> p_hom;
139  p_hom[3] = 1.0;
140 
141  for (size_t i = 0; i < indices.size(); i++) {
142  p_hom.block<3, 1>(0, 0) = opengv::triangulation::triangulate2(
143  adapterDerived_, indices[i]);
144  bearingVector_t reprojection1 = p_hom.block<3, 1>(0, 0);
145  bearingVector_t reprojection2 = inverseSolution * p_hom;
146  reprojection1 = reprojection1 / reprojection1.norm();
147  reprojection2 = reprojection2 / reprojection2.norm();
148  bearingVector_t f1 = adapterDerived_.getBearingVector1(indices[i]);
149  bearingVector_t f2 = adapterDerived_.getBearingVector2(indices[i]);
150 
151  //compute the score
152  point_t error1 = (reprojection1 - f1);
153  point_t error2 = (reprojection2 - f2);
154  double error_squared1 = error1.transpose() * error1;
155  double error_squared2 = error2.transpose() * error2;
156  scores.push_back(
157  error_squared1 * 0.5 / adapterDerived_.getSigmaAngle1(indices[i])
158  + error_squared2 * 0.5
159  / adapterDerived_.getSigmaAngle2(indices[i]));
160  }
161  }
162 
163  protected:
166 
167 };
168 
169 }
170 }
171 }
172 
173 #endif /* INCLUDE_OKVIS_OPENGV_FRAMERELATIVEPOSESACPROBLEM_HPP_ */
virtual opengv::bearingVector_t getBearingVector2(size_t index) const
Retrieve the bearing vector of a correspondence in viewpoint 2.
Definition: FrameRelativeAdapter.cpp:254
Header file for the FrameRelativeAdapter class.
opengv::relative_pose::FrameRelativeAdapter & adapterDerived_
The adapter holding the bearing, correspondences etc.
Definition: FrameRelativePoseSacProblem.hpp:165
virtual void getSelectedDistancesToModel(const model_t &model, const std::vector< int > &indices, std::vector< double > &scores) const
Compute the distances of all samples whith respect to given model coefficients.
Definition: FrameRelativePoseSacProblem.hpp:126
Adapter for relative pose RANSAC (2D2D)
Definition: FrameRelativeAdapter.hpp:60
#define OKVIS_DEFINE_EXCEPTION(exceptionName, exceptionParent)
Macro for defining an exception with a given parent.
Definition: assert_macros.hpp:52
This file contains some useful assert macros.
CentralRelativePoseSacProblem base_t
Definition: FrameRelativePoseSacProblem.hpp:72
virtual ~FrameRelativePoseSacProblem()
Definition: FrameRelativePoseSacProblem.hpp:115
double getSigmaAngle2(size_t index)
Obtain the angular standard deviation of the correspondence in frame 2 in [rad].
Definition: FrameRelativeAdapter.cpp:303
Provides functions for fitting a relative-pose model to a set of bearing-vector to point corresponden...
Definition: FrameRelativePoseSacProblem.hpp:68
virtual opengv::bearingVector_t getBearingVector1(size_t index) const
Retrieve the bearing vector of a correspondence in viewpoint 1.
Definition: FrameRelativeAdapter.cpp:248
double getSigmaAngle1(size_t index)
Obtain the angular standard deviation of the correspondence in frame 1 in [rad].
Definition: FrameRelativeAdapter.cpp:297
#define OKVIS_ASSERT_TRUE(exceptionType, condition, message)
Definition: assert_macros.hpp:111
FrameRelativePoseSacProblem(adapter_t &adapter, algorithm_t algorithm, const std::vector< int > &indices)
FrameRelativePoseSacProblem.
Definition: FrameRelativePoseSacProblem.hpp:105