OKVIS ROS
 All Classes Namespaces Files Functions Variables Typedefs Enumerations Enumerator Friends Macros Pages
Estimator.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: Jan 10, 2015
30  * Author: Stefan Leutenegger (s.leutenegger@imperial.ac.uk)
31  *********************************************************************************/
32 
39 namespace okvis {
41 
42 // Add an observation to a landmark.
43 template<class GEOMETRY_TYPE>
44 ::ceres::ResidualBlockId Estimator::addObservation(uint64_t landmarkId,
45  uint64_t poseId,
46  size_t camIdx,
47  size_t keypointIdx) {
48  OKVIS_ASSERT_TRUE_DBG(Exception, isLandmarkAdded(landmarkId),
49  "landmark not added");
50 
51  // avoid double observations
52  okvis::KeypointIdentifier kid(poseId, camIdx, keypointIdx);
53  if (landmarksMap_.at(landmarkId).observations.find(kid)
54  != landmarksMap_.at(landmarkId).observations.end()) {
55  return NULL;
56  }
57 
58  // get the keypoint measurement
59  okvis::MultiFramePtr multiFramePtr = multiFramePtrMap_.at(poseId);
60  Eigen::Vector2d measurement;
61  multiFramePtr->getKeypoint(camIdx, keypointIdx, measurement);
62  Eigen::Matrix2d information = Eigen::Matrix2d::Identity();
63  double size = 1.0;
64  multiFramePtr->getKeypointSize(camIdx, keypointIdx, size);
65  information *= 64.0 / (size * size);
66 
67  // create error term
68  std::shared_ptr < ceres::ReprojectionError
69  < GEOMETRY_TYPE
70  >> reprojectionError(
72  multiFramePtr->template geometryAs<GEOMETRY_TYPE>(camIdx),
73  camIdx, measurement, information));
74 
75  ::ceres::ResidualBlockId retVal = mapPtr_->addResidualBlock(
76  reprojectionError,
78  mapPtr_->parameterBlockPtr(poseId),
79  mapPtr_->parameterBlockPtr(landmarkId),
80  mapPtr_->parameterBlockPtr(
81  statesMap_.at(poseId).sensors.at(SensorStates::Camera).at(camIdx).at(
82  CameraSensorStates::T_SCi).id));
83 
84  // remember
85  landmarksMap_.at(landmarkId).observations.insert(
86  std::pair<okvis::KeypointIdentifier, uint64_t>(
87  kid, reinterpret_cast<uint64_t>(retVal)));
88 
89  return retVal;
90 }
91 
92 } // namespace okvis
std::map< uint64_t, States > statesMap_
Buffer for currently considered states.
Definition: Estimator.hpp:555
std::shared_ptr< okvis::ceres::Map > mapPtr_
The underlying okvis::Map.
Definition: Estimator.hpp:557
Unique identifier for a keypoint.
Definition: FrameTypedefs.hpp:58
okvis::PointMap landmarksMap_
Contains all the current landmarks (synched after optimisation).
Definition: Estimator.hpp:563
std::shared_ptr< MultiFrame > MultiFramePtr
For convenience.
Definition: MultiFrame.hpp:272
std::map< uint64_t, okvis::MultiFramePtr > multiFramePtrMap_
remember all needed okvis::MultiFrame.
Definition: Estimator.hpp:556
The 2D keypoint reprojection error.
Definition: ReprojectionError.hpp:56
bool isLandmarkAdded(uint64_t landmarkId) const
Checks whether the landmark is added to the estimator.
Definition: Estimator.hpp:218
::ceres::ResidualBlockId addObservation(uint64_t landmarkId, uint64_t poseId, size_t camIdx, size_t keypointIdx)
Add an observation to a landmark.
std::shared_ptr< ::ceres::LossFunction > cauchyLossFunctionPtr_
Cauchy loss.
Definition: Estimator.hpp:572
#define OKVIS_ASSERT_TRUE_DBG(exceptionType, condition, message)
Definition: assert_macros.hpp:211