Link.cpp
Go to the documentation of this file.
1 /*
2 Copyright (c) 2010-2016, Mathieu Labbe - IntRoLab - Universite de Sherbrooke
3 All rights reserved.
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  * Redistributions of source code must retain the above copyright
8  notice, this list of conditions and the following disclaimer.
9  * Redistributions in binary form must reproduce the above copyright
10  notice, this list of conditions and the following disclaimer in the
11  documentation and/or other materials provided with the distribution.
12  * Neither the name of the Universite de Sherbrooke nor the
13  names of its contributors may be used to endorse or promote products
14  derived from this software without specific prior written permission.
15 
16 THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
17 ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
18 WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
19 DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY
20 DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
21 (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
22 LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
23 ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
24 (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
25 SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
26 */
27 
28 #include "rtabmap/core/Link.h"
30 #include <rtabmap/utilite/UMath.h>
33 
34 namespace rtabmap {
35 
36 std::string Link::typeName(Type type)
37 {
38  if(type == Link::kNeighbor)
39  return "Neighbor";
41  return "GlobalClosure";
43  return "LocalSpaceClosure";
45  return "LocalTimeClosure";
47  return "UserClosure";
49  return "VirtualClosure";
51  return "NeighborMerged";
52  if(type == Link::kPosePrior)
53  return "PosePrior";
54  if(type == Link::kLandmark)
55  return "Landmark";
56  if(type == Link::kGravity)
57  return "Gravity";
58  return "Undefined";
59 }
60 
62  from_(0),
63  to_(0),
64  type_(kUndef),
65  infMatrix_(cv::Mat::eye(6,6,CV_64FC1))
66 {
67 }
68 Link::Link(int from,
69  int to,
70  Type type,
71  const Transform & transform,
72  const cv::Mat & infMatrix,
73  const cv::Mat & userData) :
74  from_(from),
75  to_(to),
77  type_(type)
78 {
80 
81  if(userData.type() == CV_8UC1) // Bytes
82  {
83  _userDataCompressed = userData; // assume compressed
84  }
85  else
86  {
87  _userDataRaw = userData;
89  }
90 }
91 
92 double Link::rotVariance(bool minimum) const
93 {
94  double value;
95  if(minimum)
96  {
97  value = uMax3(infMatrix_.at<double>(3,3), infMatrix_.at<double>(4,4), infMatrix_.at<double>(5,5));
98  }
99  else
100  {
101  value = uMin3(
102  infMatrix_.at<double>(3,3) <= 0.0001?9999999.0:infMatrix_.at<double>(3,3),
103  infMatrix_.at<double>(4,4) <= 0.0001?9999999.0:infMatrix_.at<double>(4,4),
104  infMatrix_.at<double>(5,5) <= 0.0001?9999999.0:infMatrix_.at<double>(5,5));
105  if(value == 9999999.0)
106  {
107  value = 0.0001;
108  }
109  }
110  UASSERT(value > 0.0);
111  return 1.0/value;
112 }
113 double Link::transVariance(bool minimum) const
114 {
115  double value;
116  if(minimum)
117  {
118  value = uMax3(infMatrix_.at<double>(0,0), infMatrix_.at<double>(1,1), infMatrix_.at<double>(2,2));
119  }
120  else
121  {
122  value = uMin3(
123  infMatrix_.at<double>(0,0) <= 0.0001?9999999.0:infMatrix_.at<double>(0,0),
124  infMatrix_.at<double>(1,1) <= 0.0001?9999999.0:infMatrix_.at<double>(1,1),
125  infMatrix_.at<double>(2,2) <= 0.0001?9999999.0:infMatrix_.at<double>(2,2));
126  if(value == 9999999.0)
127  {
128  value = 0.0001;
129  }
130  }
131  UASSERT(value > 0.0);
132  return 1.0/value;
133 }
134 
135 void Link::setInfMatrix(const cv::Mat & infMatrix) {
136  UASSERT(infMatrix.cols == 6 && infMatrix.rows == 6 && infMatrix.type() == CV_64FC1);
137  UASSERT_MSG(uIsFinite(infMatrix.at<double>(0,0)) && infMatrix.at<double>(0,0)>0, uFormat("Linear information X should not be null! Value=%f (set to 1 if unknown or <=1/9999 to be ignored in some computations).", infMatrix.at<double>(0,0)).c_str());
138  UASSERT_MSG(uIsFinite(infMatrix.at<double>(1,1)) && infMatrix.at<double>(1,1)>0, uFormat("Linear information Y should not be null! Value=%f (set to 1 if unknown or <=1/9999 to be ignored in some computations).", infMatrix.at<double>(1,1)).c_str());
139  UASSERT_MSG(uIsFinite(infMatrix.at<double>(2,2)) && infMatrix.at<double>(2,2)>0, uFormat("Linear information Z should not be null! Value=%f (set to 1 if unknown or <=1/9999 to be ignored in some computations).", infMatrix.at<double>(2,2)).c_str());
140  UASSERT_MSG(uIsFinite(infMatrix.at<double>(3,3)) && infMatrix.at<double>(3,3)>0, uFormat("Angular information roll should not be null! Value=%f (set to 1 if unknown or <=1/9999 to be ignored in some computations).", infMatrix.at<double>(3,3)).c_str());
141  UASSERT_MSG(uIsFinite(infMatrix.at<double>(4,4)) && infMatrix.at<double>(4,4)>0, uFormat("Angular information pitch should not be null! Value=%f (set to 1 if unknown or <=1/9999 to be ignored in some computations).", infMatrix.at<double>(4,4)).c_str());
142  UASSERT_MSG(uIsFinite(infMatrix.at<double>(5,5)) && infMatrix.at<double>(5,5)>0, uFormat("Angular information yaw should not be null! Value=%f (set to 1 if unknown or <=1/9999 to be ignored in some computations).", infMatrix.at<double>(5,5)).c_str());
144 }
145 
147 {
148  cv::Mat dataRaw = uncompressUserDataConst();
149  if(!dataRaw.empty() && _userDataRaw.empty())
150  {
151  _userDataRaw = dataRaw;
152  }
153 }
154 
156 {
157  if(!_userDataRaw.empty())
158  {
159  return _userDataRaw;
160  }
162 }
163 
164 Link Link::merge(const Link & link, Type outputType) const
165 {
166  UASSERT_MSG(to_ == link.from(), uFormat("merging this=%d->%d to link=%d->%d", from_, to_, link.from(), link.to()).c_str());
167  UASSERT(outputType != Link::kUndef);
168  UASSERT((link.transform().isNull() && transform_.isNull()) || (!link.transform().isNull() && !transform_.isNull()));
169  UASSERT(infMatrix_.cols == 6 && infMatrix_.rows == 6 && infMatrix_.type() == CV_64FC1);
170  UASSERT(link.infMatrix().cols == 6 && link.infMatrix().rows == 6 && link.infMatrix().type() == CV_64FC1);
171  if(outputType == kNeighborMerged)
172  {
173  return Link(
174  from_,
175  link.to(),
176  outputType,
178  transform_.isNull()?cv::Mat::eye(6,6,CV_64FC1):(infMatrix_.inv() + link.infMatrix().inv()).inv());
179  }
180  return Link(
181  from_,
182  link.to(),
183  outputType,
184  transform_.isNull()?Transform():transform_ * link.transform(), // FIXME, should be inf1^-1(inf1*t1 + inf2*t2)
185  transform_.isNull()?cv::Mat::eye(6,6,CV_64FC1):(infMatrix_.at<double>(0,0)<link.infMatrix().at<double>(0,0)?infMatrix_:link.infMatrix()));
186  //transform_.isNull()?cv::Mat::eye(6,6,CV_64FC1):(infMatrix_.inv() + link.infMatrix().inv()).inv());
187 }
188 
190 {
191  return Link(
192  to_,
193  from_,
194  type_,
196  transform_.isNull()?cv::Mat::eye(6,6,CV_64FC1):infMatrix_);
197 }
198 
199 }
Compression.h
uMin3
T uMin3(const T &a, const T &b, const T &c)
Definition: UMath.h:67
type
transform_
OrientedPlane3 transform_(const OrientedPlane3 &plane, const Pose3 &xr)
rtabmap::Transform::isNull
bool isNull() const
Definition: Transform.cpp:107
UMath.h
Basic mathematics functions.
Type
UConversion.h
Some conversion functions.
uMax3
T uMax3(const T &a, const T &b, const T &c)
Definition: UMath.h:78
UASSERT
#define UASSERT(condition)
rtabmap::Transform::inverse
Transform inverse() const
Definition: Transform.cpp:178
UASSERT_MSG
#define UASSERT_MSG(condition, msg_str)
Definition: ULogger.h:67
rtabmap::compressData2
cv::Mat RTABMAP_CORE_EXPORT compressData2(const cv::Mat &data)
Definition: Compression.cpp:201
uFormat
std::string UTILITE_EXPORT uFormat(const char *fmt,...)
Definition: UConversion.cpp:365
ULogger.h
ULogger class and convenient macros.
rtabmap::Transform
Definition: Transform.h:41
Matrix< Scalar, Dynamic, Dynamic >
cv
Definition: Features2d.h:41
transform
EIGEN_DONT_INLINE void transform(const Quaternion< Scalar > &t, Data &data)
rtabmap
Definition: CameraARCore.cpp:35
value
value
uIsFinite
bool uIsFinite(const T &value)
Definition: UMath.h:53
rtabmap::uncompressData
cv::Mat RTABMAP_CORE_EXPORT uncompressData(const cv::Mat &bytes)
Definition: Compression.cpp:231


rtabmap
Author(s): Mathieu Labbe
autogenerated on Thu Jul 25 2024 02:50:11