OKVIS ROS
 All Classes Namespaces Files Functions Variables Typedefs Enumerations Enumerator Friends Macros Pages
FrameRotationOnlySacProblem.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 15, 2013
30  * Author: Stefan Leutenegger (s.leutenegger@imperial.ac.uk)
31  *********************************************************************************/
32 
39 #ifndef INCLUDE_OKVIS_OPENGV_FRAMEROTATIONONLYSACPROBLEM_HPP_
40 #define INCLUDE_OKVIS_OPENGV_FRAMEROTATIONONLYSACPROBLEM_HPP_
41 
42 #include <okvis/assert_macros.hpp>
43 #include <opengv/types.hpp>
44 #include <opengv/relative_pose/methods.hpp>
45 #include <opengv/sac_problems/relative_pose/RotationOnlySacProblem.hpp>
47 
51 namespace opengv {
55 namespace sac_problems {
59 namespace relative_pose {
60 
67 class FrameRotationOnlySacProblem : public RotationOnlySacProblem {
68  public:
69  OKVIS_DEFINE_EXCEPTION(Exception,std::runtime_error)
70 
71  typedef RotationOnlySacProblem base_t;
72 
74  using base_t::adapter_t;
76  using base_t::model_t;
77 
83  FrameRotationOnlySacProblem(adapter_t & adapter)
84  : base_t(adapter),
86  *static_cast<opengv::relative_pose::FrameRelativeAdapter*>(&_adapter)) {
88  Exception,
89  dynamic_cast<opengv::relative_pose::FrameRelativeAdapter*>(&_adapter),
90  "only opengv::absolute_pose::FrameRelativeAdapter supported");
91  }
92 
100  FrameRotationOnlySacProblem(adapter_t & adapter,
101  const std::vector<int> & indices)
102  : base_t(adapter, indices),
104  *static_cast<opengv::relative_pose::FrameRelativeAdapter*>(&_adapter)) {
106  Exception,
107  dynamic_cast<opengv::relative_pose::FrameRelativeAdapter*>(&_adapter),
108  "only opengv::absolute_pose::FrameRelativeAdapter supported");
109  }
110 
112  }
113 
122  virtual void getSelectedDistancesToModel(const model_t & model,
123  const std::vector<int> & indices,
124  std::vector<double> & scores) const {
125  for (size_t i = 0; i < indices.size(); i++) {
126  bearingVector_t f1 = adapterDerived_.getBearingVector1(indices[i]);
127  bearingVector_t f2 = adapterDerived_.getBearingVector2(indices[i]);
128 
129  //unrotate bearing-vector f2
130  bearingVector_t f2_unrotated = model * f2;
131 
132  //unrotate bearing-vector f1
133  bearingVector_t f1_unrotated = model.transpose() * f1;
134 
135  point_t error1 = (f2_unrotated - f1);
136  point_t error2 = (f1_unrotated - f2);
137  double error_squared1 = error1.transpose() * error1;
138  double error_squared2 = error2.transpose() * error2;
139  scores.push_back(
140  error_squared1 * 0.5 / adapterDerived_.getSigmaAngle1(indices[i])
141  + error_squared2 * 0.5
142  / adapterDerived_.getSigmaAngle2(indices[i]));
143  }
144  }
145 
146  protected:
149 
150 };
151 
152 }
153 }
154 }
155 
156 #endif /* INCLUDE_OKVIS_OPENGV_FRAMEROTATIONONLYPOSESACPROBLEM_HPP_ */
virtual opengv::bearingVector_t getBearingVector2(size_t index) const
Retrieve the bearing vector of a correspondence in viewpoint 2.
Definition: FrameRelativeAdapter.cpp:254
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: FrameRotationOnlySacProblem.hpp:122
Functions for fitting a rotation-only model to a set of bearing-vector correspondences (using twopt_r...
Definition: FrameRotationOnlySacProblem.hpp:67
Header file for the FrameRelativeAdapter class.
Adapter for relative pose RANSAC (2D2D)
Definition: FrameRelativeAdapter.hpp:60
opengv::relative_pose::FrameRelativeAdapter & adapterDerived_
The adapter holding the bearing, correspondences etc.
Definition: FrameRotationOnlySacProblem.hpp:148
#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.
FrameRotationOnlySacProblem(adapter_t &adapter, const std::vector< int > &indices)
Constructor.
Definition: FrameRotationOnlySacProblem.hpp:100
virtual ~FrameRotationOnlySacProblem()
Definition: FrameRotationOnlySacProblem.hpp:111
double getSigmaAngle2(size_t index)
Obtain the angular standard deviation of the correspondence in frame 2 in [rad].
Definition: FrameRelativeAdapter.cpp:303
RotationOnlySacProblem base_t
Definition: FrameRotationOnlySacProblem.hpp:71
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