OKVIS ROS
 All Classes Namespaces Files Functions Variables Typedefs Enumerations Enumerator Friends Macros Pages
FrameAbsolutePoseSacProblem.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 6, 2013
30  * Author: Stefan Leutenegger (s.leutenegger@imperial.ac.uk)
31  *********************************************************************************/
32 
39 #ifndef INCLUDE_OKVIS_OPENGV_FRAMEABSOLUTEPOSESACPROBLEM_HPP_
40 #define INCLUDE_OKVIS_OPENGV_FRAMEABSOLUTEPOSESACPROBLEM_HPP_
41 
42 #include <opengv/types.hpp>
43 #include <opengv/absolute_pose/methods.hpp>
44 #pragma GCC diagnostic push
45 #pragma GCC diagnostic ignored "-Wunused-parameter"
46 #include <opengv/sac_problems/absolute_pose/AbsolutePoseSacProblem.hpp>
47 #pragma GCC diagnostic pop
49 #include <okvis/assert_macros.hpp>
50 
54 namespace opengv {
58 namespace sac_problems {
62 namespace absolute_pose {
63 
70 class FrameAbsolutePoseSacProblem : public AbsolutePoseSacProblem {
71  public:
72  OKVIS_DEFINE_EXCEPTION(Exception,std::runtime_error)
73 
74  typedef AbsolutePoseSacProblem base_t;
75 
77  using base_t::adapter_t;
79  using base_t::algorithm_t;
81  using base_t::model_t;
82 
89  FrameAbsolutePoseSacProblem(adapter_t & adapter, algorithm_t algorithm)
90  : base_t(adapter, algorithm),
92  *static_cast<opengv::absolute_pose::FrameNoncentralAbsoluteAdapter*>(&_adapter)) {
94  Exception,
95  dynamic_cast<opengv::absolute_pose::FrameNoncentralAbsoluteAdapter*>(&_adapter),
96  "only opengv::absolute_pose::FrameNoncentralAbsoluteAdapter supported");
97  }
98 
107  FrameAbsolutePoseSacProblem(adapter_t & adapter, algorithm_t algorithm,
108  const std::vector<int> & indices)
109  : base_t(adapter, algorithm, indices),
111  *static_cast<opengv::absolute_pose::FrameNoncentralAbsoluteAdapter*>(&_adapter)) {
113  Exception,
114  dynamic_cast<opengv::absolute_pose::FrameNoncentralAbsoluteAdapter*>(&_adapter),
115  "only opengv::absolute_pose::FrameNoncentralAbsoluteAdapter supported");
116  }
117 
119  }
120 
129  virtual void getSelectedDistancesToModel(const model_t & model,
130  const std::vector<int> & indices,
131  std::vector<double> & scores) const {
132  //compute the reprojection error of all points
133 
134  //compute inverse transformation
135  model_t inverseSolution;
136  inverseSolution.block<3, 3>(0, 0) = model.block<3, 3>(0, 0).transpose();
137  inverseSolution.col(3) = -inverseSolution.block<3, 3>(0, 0) * model.col(3);
138 
139  Eigen::Matrix<double, 4, 1> p_hom;
140  p_hom[3] = 1.0;
141 
142  for (size_t i = 0; i < indices.size(); i++) {
143  //get point in homogeneous form
144  p_hom.block<3, 1>(0, 0) = adapterDerived_.getPoint(indices[i]);
145 
146  //compute the reprojection (this is working for both central and
147  //non-central case)
148  point_t bodyReprojection = inverseSolution * p_hom;
149  point_t reprojection = adapterDerived_.getCamRotation(indices[i])
150  .transpose()
151  * (bodyReprojection - adapterDerived_.getCamOffset(indices[i]));
152  reprojection = reprojection / reprojection.norm();
153 
154  //compute the score
155  point_t error = (reprojection
156  - adapterDerived_.getBearingVector(indices[i]));
157  double error_squared = error.transpose() * error;
158  scores.push_back(
159  error_squared / adapterDerived_.getSigmaAngle(indices[i]));
160  }
161  }
162 
163  protected:
166 
167 };
168 
169 }
170 }
171 }
172 
173 #endif /* INCLUDE_OKVIS_OPENGV_FRAMEABSOLUTEPOSESACPROBLEM_HPP_ */
double getSigmaAngle(size_t index)
Obtain the angular standard deviation in [rad].
Definition: FrameNoncentralAbsoluteAdapter.cpp:198
virtual ~FrameAbsolutePoseSacProblem()
Definition: FrameAbsolutePoseSacProblem.hpp:118
virtual opengv::rotation_t getCamRotation(size_t index) const
Retrieve the rotation from a camera of a correspondence to the viewpoint origin.
Definition: FrameNoncentralAbsoluteAdapter.cpp:185
Header file for the FrameNoncentralAbsoluteAdapter class.
virtual opengv::bearingVector_t getBearingVector(size_t index) const
Retrieve the bearing vector of a correspondence.
Definition: FrameNoncentralAbsoluteAdapter.cpp:165
#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.
virtual opengv::point_t getPoint(size_t index) const
Retrieve the world point of a correspondence.
Definition: FrameNoncentralAbsoluteAdapter.cpp:172
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: FrameAbsolutePoseSacProblem.hpp:129
Adapter for absolute pose RANSAC (3D2D) with non-central cameras, i.e. could be a multi-camera-setup...
Definition: FrameNoncentralAbsoluteAdapter.hpp:63
FrameAbsolutePoseSacProblem(adapter_t &adapter, algorithm_t algorithm, const std::vector< int > &indices)
Constructor.
Definition: FrameAbsolutePoseSacProblem.hpp:107
AbsolutePoseSacProblem base_t
Definition: FrameAbsolutePoseSacProblem.hpp:74
virtual opengv::translation_t getCamOffset(size_t index) const
Retrieve the position of a camera of a correspondence seen from the viewpoint origin.
Definition: FrameNoncentralAbsoluteAdapter.cpp:179
opengv::absolute_pose::FrameNoncentralAbsoluteAdapter & adapterDerived_
The adapter holding the bearing, correspondences etc.
Definition: FrameAbsolutePoseSacProblem.hpp:165
Provides functions for fitting an absolute-pose model to a set of bearing-vector to point corresponde...
Definition: FrameAbsolutePoseSacProblem.hpp:70
#define OKVIS_ASSERT_TRUE(exceptionType, condition, message)
Definition: assert_macros.hpp:111