OKVIS
 All Classes Namespaces Files Functions Variables Typedefs Enumerations Enumerator Friends Macros Pages
ProbabilisticStereoTriangulator.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: Oct 17, 2013
30  * Author: Stefan Leutenegger (s.leutenegger@imperial.ac.uk)
31  * Modified: Andreas Forster (an.forster@gmail.com)
32  *********************************************************************************/
33 
41 #ifndef INCLUDE_OKVIS_TRIANGULATION_PROBABILISTICSTEREOTRIANGULATOR_HPP_
42 #define INCLUDE_OKVIS_TRIANGULATION_PROBABILISTICSTEREOTRIANGULATOR_HPP_
43 
44 #include <memory>
45 
46 #include <Eigen/Core>
48 #include <okvis/assert_macros.hpp>
49 #include <okvis/FrameTypedefs.hpp>
51 #include <okvis/MultiFrame.hpp>
52 
54 namespace okvis {
55 namespace triangulation {
56 
61 template<class CAMERA_GEOMETRY_T>
63  public:
64  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
65  OKVIS_DEFINE_EXCEPTION(Exception, std::runtime_error)
66 
67 
72  ProbabilisticStereoTriangulator(double pixelSigma = 0.5);
73 
85  ProbabilisticStereoTriangulator(std::shared_ptr<okvis::MultiFrame> frameA_ptr,
86  std::shared_ptr<okvis::MultiFrame> frameB_ptr,
87  size_t camIdA, size_t camIdB,
89  const Eigen::Matrix<double, 6, 6>& UOplus,
90  double pixelSigma = 0.5);
91 
101  void resetFrames(std::shared_ptr<okvis::MultiFrame> frameA_ptr,
102  std::shared_ptr<okvis::MultiFrame> frameB_ptr, size_t camIdA,
103  size_t camIdB, const okvis::kinematics::Transformation& T_AB,
104  const Eigen::Matrix<double, 6, 6>& UOplus);
105 
108 
119  bool stereoTriangulate(size_t keypointIdxA, size_t keypointIdxB,
120  Eigen::Vector4d & outHomogeneousPoint_A,
121  bool & outCanBeInitializedInaccuarate,
122  double sigmaRay = -1.0) const;
123 
134  bool stereoTriangulate(size_t keypointIdxA, size_t keypointIdxB,
135  Eigen::Vector4d & outHomogeneousPoint_A,
136  Eigen::Matrix3d & outPointUOplus_A,
137  bool & outCanBeInitialized,
138  double sigmaRay = -1.0) const;
139 
148  void getUncertainty(size_t keypointIdxA, size_t keypointIdxB,
149  const Eigen::Vector4d & homogeneousPoint_A,
150  Eigen::Matrix3d& outPointUOplus_A,
151  bool & outCanBeInitialized) const;
152 
153  protected:
154  double sigmaRay_;
155  std::shared_ptr<okvis::MultiFrame> frameA_;
158  std::shared_ptr<okvis::MultiFrame> frameB_;
159 
161  size_t camIdA_;
163  size_t camIdB_;
164 
173 
175  Eigen::Matrix<double, 6, 6> UOplus_;
176 
178  Eigen::Matrix<double, 9, 9> H_;
179 
190  const std::shared_ptr<okvis::MultiFrame> &frame, size_t camId,
191  size_t keypointId, const Eigen::Vector4d& homogeneousPoint,
192  double& outError) const;
193 };
194 
195 } // namespace triangulation
196 } // namespace okvis
197 
198 #endif /* INCLUDE_OKVIS_TRIANGULATION_PROBABILISTICSTEREOTRIANGULATOR_HPP_ */
The ProbabilisticStereoTriangulator class.
Definition: ProbabilisticStereoTriangulator.hpp:62
okvis::kinematics::Transformation T_AB_
Definition: ProbabilisticStereoTriangulator.hpp:167
size_t camIdB_
Camera ID for frame B.
Definition: ProbabilisticStereoTriangulator.hpp:163
~ProbabilisticStereoTriangulator()
Default destructor.
Definition: ProbabilisticStereoTriangulator.cpp:163
Header file for the PoseParameterBlock class.
Header file for the MultiFrame class.
void getUncertainty(size_t keypointIdxA, size_t keypointIdxB, const Eigen::Vector4d &homogeneousPoint_A, Eigen::Matrix3d &outPointUOplus_A, bool &outCanBeInitialized) const
Get triangulation uncertainty.
Definition: ProbabilisticStereoTriangulator.cpp:253
double sigmaRay_
Definition: ProbabilisticStereoTriangulator.hpp:154
::okvis::ceres::PoseParameterBlock poseB_
Definition: ProbabilisticStereoTriangulator.hpp:169
Eigen::Matrix< double, 6, 6 > UOplus_
Local copy of relative uncertainty.
Definition: ProbabilisticStereoTriangulator.hpp:175
Wraps the parameter block for a pose estimate.
Definition: PoseParameterBlock.hpp:52
A class that does homogeneous transformations. This relates a frame A and B: T_AB; it consists of tra...
Definition: Transformation.hpp:76
std::shared_ptr< okvis::MultiFrame > frameA_
The multiframe A.
Definition: ProbabilisticStereoTriangulator.hpp:156
EIGEN_MAKE_ALIGNED_OPERATOR_NEW ProbabilisticStereoTriangulator(double pixelSigma=0.5)
Default constructor; make sure to call resetFrames before triangulation!
Definition: ProbabilisticStereoTriangulator.cpp:60
bool stereoTriangulate(size_t keypointIdxA, size_t keypointIdxB, Eigen::Vector4d &outHomogeneousPoint_A, bool &outCanBeInitializedInaccuarate, double sigmaRay=-1.0) const
Triangulation.
Definition: ProbabilisticStereoTriangulator.cpp:168
#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.
void resetFrames(std::shared_ptr< okvis::MultiFrame > frameA_ptr, std::shared_ptr< okvis::MultiFrame > frameB_ptr, size_t camIdA, size_t camIdB, const okvis::kinematics::Transformation &T_AB, const Eigen::Matrix< double, 6, 6 > &UOplus)
Reset frames and relative transformation.
Definition: ProbabilisticStereoTriangulator.cpp:117
::okvis::ceres::PoseParameterBlock poseA_
Definition: ProbabilisticStereoTriangulator.hpp:168
bool computeReprojectionError4(const std::shared_ptr< okvis::MultiFrame > &frame, size_t camId, size_t keypointId, const Eigen::Vector4d &homogeneousPoint, double &outError) const
Compute the reprojection error.
Definition: ProbabilisticStereoTriangulator.cpp:359
Eigen::Matrix< double, 9, 9 > H_
Information matrix of pose and landmark.
Definition: ProbabilisticStereoTriangulator.hpp:178
::okvis::ceres::PoseParameterBlock extrinsics_
Definition: ProbabilisticStereoTriangulator.hpp:170
Header file for the Transformation class.
std::shared_ptr< okvis::MultiFrame > frameB_
The multiframe B.
Definition: ProbabilisticStereoTriangulator.hpp:158
size_t camIdA_
Camera ID for frame A.
Definition: ProbabilisticStereoTriangulator.hpp:161
This file contains useful typedefs and structs related to frames.
okvis::kinematics::Transformation T_BA_
Definition: ProbabilisticStereoTriangulator.hpp:171