OdometryViso2.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 
30 #include "rtabmap/core/util2d.h"
32 #include "rtabmap/utilite/UTimer.h"
33 #include "rtabmap/utilite/UStl.h"
34 #include <opencv2/imgproc/types_c.h>
35 
36 #ifdef RTABMAP_VISO2
37 #include <viso_stereo.h>
38 
39 double computeFeatureFlow(const std::vector<Matcher::p_match>& matches)
40  {
41  double total_flow = 0.0;
42  for (size_t i = 0; i < matches.size(); ++i)
43  {
44  double x_diff = matches[i].u1c - matches[i].u1p;
45  double y_diff = matches[i].v1c - matches[i].v1p;
46  total_flow += sqrt(x_diff * x_diff + y_diff * y_diff);
47  }
48  return total_flow / matches.size();
49 }
50 #endif
51 
52 namespace rtabmap {
53 
55  Odometry(parameters),
56 #ifdef RTABMAP_VISO2
57  viso2_(0),
58  ref_frame_change_method_(0),
59  ref_frame_inlier_threshold_(Parameters::defaultOdomVisKeyFrameThr()),
60  ref_frame_motion_threshold_(5.0),
61  lost_(false),
62  keep_reference_frame_(false),
63 #endif
64  reference_motion_(Transform::getIdentity())
65 {
66 #ifdef RTABMAP_VISO2
67  Parameters::parse(parameters, Parameters::kOdomVisKeyFrameThr(), ref_frame_inlier_threshold_);
68 #endif
69  viso2Parameters_ = Parameters::filterParameters(parameters, "OdomViso2");
70 }
71 
73 {
74 #ifdef RTABMAP_VISO2
75  delete viso2_;
76 #endif
77 }
78 
79 void OdometryViso2::reset(const Transform & initialPose)
80 {
81  Odometry::reset(initialPose);
82 #ifdef RTABMAP_VISO2
83  if(viso2_)
84  {
85  delete viso2_;
86  viso2_ = 0;
87  }
88  lost_ = false;
91 #endif
92 }
93 
94 // return not null transform if odometry is correctly computed
96  SensorData & data,
97  const Transform & guess,
98  OdometryInfo * info)
99 {
100  Transform t;
101 #ifdef RTABMAP_VISO2
102  //based on https://github.com/srv/viso2/blob/indigo/viso2_ros/src/stereo_odometer.cpp
103 
104  UTimer timer;
105 
106  if(!data.depthRaw().empty())
107  {
108  UERROR("viso2 odometry doesn't support RGB-D data, only stereo. Aborting odometry update...");
109  return t;
110  }
111 
112  if(data.imageRaw().empty() ||
113  data.imageRaw().rows != data.rightRaw().rows ||
114  data.imageRaw().cols != data.rightRaw().cols)
115  {
116  UERROR("Not compatible left (%dx%d) or right (%dx%d) image.",
117  data.imageRaw().rows,
118  data.imageRaw().cols,
119  data.rightRaw().rows,
120  data.rightRaw().cols);
121  return t;
122  }
123 
124  if(!(data.stereoCameraModels().size() == 1 &&
125  data.stereoCameraModels()[0].isValidForProjection()))
126  {
127  UERROR("Invalid stereo camera model!");
128  return t;
129  }
130 
131  cv::Mat leftGray;
132  if(data.imageRaw().type() == CV_8UC3)
133  {
134  cv::cvtColor(data.imageRaw(), leftGray, CV_BGR2GRAY);
135  }
136  else if(data.imageRaw().type() == CV_8UC1)
137  {
138  leftGray = data.imageRaw();
139  }
140  else
141  {
142  UFATAL("Not supported color type!");
143  }
144  cv::Mat rightGray;
145  if(data.rightRaw().type() == CV_8UC3)
146  {
147  cv::cvtColor(data.rightRaw(), rightGray, CV_BGR2GRAY);
148  }
149  else if(data.rightRaw().type() == CV_8UC1)
150  {
151  rightGray = data.rightRaw();
152  }
153  else
154  {
155  UFATAL("Not supported color type!");
156  }
157 
158  int32_t dims[] = {leftGray.cols, leftGray.rows, leftGray.cols};
159  cv::Mat covariance;
160  if(viso2_ == 0)
161  {
162  VisualOdometryStereo::parameters params;
163  params.base = params.match.base = data.stereoCameraModels()[0].baseline();
164  params.calib.cu = params.match.cu = data.stereoCameraModels()[0].left().cx();
165  params.calib.cv = params.match.cv = data.stereoCameraModels()[0].left().cy();
166  params.calib.f = params.match.f = data.stereoCameraModels()[0].left().fx();
167 
168  Parameters::parse(viso2Parameters_, Parameters::kOdomViso2RansacIters(), params.ransac_iters);
169  Parameters::parse(viso2Parameters_, Parameters::kOdomViso2InlierThreshold(), params.inlier_threshold);
170  Parameters::parse(viso2Parameters_, Parameters::kOdomViso2Reweighting(), params.reweighting);
171 
172  Parameters::parse(viso2Parameters_, Parameters::kOdomViso2MatchNmsN(), params.match.nms_n);
173  Parameters::parse(viso2Parameters_, Parameters::kOdomViso2MatchNmsTau(), params.match.nms_tau);
174  Parameters::parse(viso2Parameters_, Parameters::kOdomViso2MatchBinsize(), params.match.match_binsize);
175  Parameters::parse(viso2Parameters_, Parameters::kOdomViso2MatchRadius(), params.match.match_radius);
176  Parameters::parse(viso2Parameters_, Parameters::kOdomViso2MatchDispTolerance(), params.match.match_disp_tolerance);
177  Parameters::parse(viso2Parameters_, Parameters::kOdomViso2MatchOutlierDispTolerance(), params.match.outlier_disp_tolerance);
178  Parameters::parse(viso2Parameters_, Parameters::kOdomViso2MatchOutlierFlowTolerance(), params.match.outlier_flow_tolerance);
179  bool multistage = Parameters::defaultOdomViso2MatchMultiStage();
180  bool halfResolution = Parameters::defaultOdomViso2MatchHalfResolution();
181  Parameters::parse(viso2Parameters_, Parameters::kOdomViso2MatchMultiStage(), multistage);
182  Parameters::parse(viso2Parameters_, Parameters::kOdomViso2MatchHalfResolution() , halfResolution);
183  params.match.multi_stage = multistage?1:0;
184  params.match.half_resolution = halfResolution?1:0;
185  Parameters::parse(viso2Parameters_, Parameters::kOdomViso2MatchRefinement(), params.match.refinement);
186 
187  Parameters::parse(viso2Parameters_, Parameters::kOdomViso2BucketMaxFeatures(), params.bucket.max_features);
188  Parameters::parse(viso2Parameters_, Parameters::kOdomViso2BucketWidth(), params.bucket.bucket_width);
189  Parameters::parse(viso2Parameters_, Parameters::kOdomViso2BucketHeight(), params.bucket.bucket_height);
190 
191  viso2_ = new VisualOdometryStereo(params);
192 
193  viso2_->process(leftGray.data, rightGray.data, dims);
194  t.setIdentity();
195  covariance = cv::Mat::eye(6,6, CV_64FC1)*9999.0;
196  }
197  else
198  {
199  bool success = viso2_->process(leftGray.data, rightGray.data, dims, lost_ || keep_reference_frame_);
200  if (success)
201  {
202  Matrix motionViso = Matrix::inv(viso2_->getMotion());
203  Transform motion(motionViso.val[0][0], motionViso.val[0][1], motionViso.val[0][2],motionViso.val[0][3],
204  motionViso.val[1][0], motionViso.val[1][1], motionViso.val[1][2],motionViso.val[1][3],
205  motionViso.val[2][0], motionViso.val[2][1], motionViso.val[2][2], motionViso.val[2][3]);
206  Transform camera_motion;
207 
208  if(lost_ || keep_reference_frame_)
209  {
210  camera_motion = reference_motion_.inverse() * motion;
211  }
212  else
213  {
214  camera_motion = motion;
215  }
216  reference_motion_ = motion; // store last motion as reference
217 
218  t=camera_motion;
219 
220  //based on values set in viso2_ros
221  covariance = cv::Mat::eye(6,6, CV_64FC1);
222  covariance.at<double>(0,0) = 0.002;
223  covariance.at<double>(1,1) = 0.002;
224  covariance.at<double>(2,2) = 0.05;
225  covariance.at<double>(3,3) = 0.09;
226  covariance.at<double>(4,4) = 0.09;
227  covariance.at<double>(5,5) = 0.09;
228 
229  lost_=false;
230  }
231  else
232  {
233  covariance = cv::Mat::eye(6,6, CV_64FC1)*9999.0;
234  lost_ = true;
235  }
236 
237  if(success)
238  {
239  // Proceed depending on the reference frame change method
240  if(ref_frame_change_method_==1)
241  {
242  // calculate current feature flow
243  double feature_flow = computeFeatureFlow(viso2_->getMatches());
244  keep_reference_frame_ = (feature_flow < ref_frame_motion_threshold_);
245  }
246  else
247  {
248  keep_reference_frame_ = ref_frame_inlier_threshold_==0 || viso2_->getNumberOfInliers() > ref_frame_inlier_threshold_;
249  }
250  }
251  else
252  {
253  keep_reference_frame_ = false;
254  }
255  }
256 
257  const Transform & localTransform = data.stereoCameraModels()[0].localTransform();
258  if(!t.isNull() && !t.isIdentity() && !localTransform.isIdentity() && !localTransform.isNull())
259  {
260  // from camera frame to base frame
262  {
263  t = previousLocalTransform_ * t * localTransform.inverse();
264  }
265  else
266  {
267  t = localTransform * t * localTransform.inverse();
268  }
269  previousLocalTransform_ = localTransform;
270  }
271 
272  if(info)
273  {
274  info->type = (int)kTypeViso2;
275  info->keyFrameAdded = !keep_reference_frame_;
276  info->reg.matches = viso2_->getNumberOfMatches();
277  info->reg.inliers = viso2_->getNumberOfInliers();
278  if(covariance.cols == 6 && covariance.rows == 6 && covariance.type() == CV_64FC1)
279  {
280  info->reg.covariance = covariance;
281  }
282 
283  if(this->isInfoDataFilled())
284  {
285  std::vector<Matcher::p_match> matches = viso2_->getMatches();
286  info->refCorners.resize(matches.size());
287  info->newCorners.resize(matches.size());
288  info->cornerInliers.resize(matches.size());
289  for (size_t i = 0; i < matches.size(); ++i)
290  {
291  info->refCorners[i].x = matches[i].u1p;
292  info->refCorners[i].y = matches[i].v1p;
293  info->newCorners[i].x = matches[i].u1c;
294  info->newCorners[i].y = matches[i].v1c;
295  info->cornerInliers[i] = i;
296  }
297  }
298  }
299 
300  UINFO("Odom update time = %fs lost=%s", timer.elapsed(), lost_?"true":"false");
301 
302 #else
303  UERROR("RTAB-Map is not built with VISO2 support! Select another visual odometry approach.");
304 #endif
305  return t;
306 }
307 
308 } // namespace rtabmap
rtabmap::SensorData
Definition: SensorData.h:51
int
int
UINFO
#define UINFO(...)
rtabmap::OdometryViso2::viso2Parameters_
ParametersMap viso2Parameters_
Definition: OdometryViso2.h:60
OdometryInfo.h
timer
rtabmap::OdometryViso2::reset
virtual void reset(const Transform &initialPose=Transform::getIdentity())
Definition: OdometryViso2.cpp:79
OdometryViso2.h
rtabmap::Transform::setNull
void setNull()
Definition: Transform.cpp:152
rtabmap::ParametersMap
std::map< std::string, std::string > ParametersMap
Definition: Parameters.h:43
UTimer.h
rtabmap::Transform::isNull
bool isNull() const
Definition: Transform.cpp:107
rtabmap::OdometryViso2::reference_motion_
Transform reference_motion_
Definition: OdometryViso2.h:58
rtabmap::Parameters::parse
static bool parse(const ParametersMap &parameters, const std::string &key, bool &value)
Definition: Parameters.cpp:500
rtabmap::Odometry::kTypeViso2
@ kTypeViso2
Definition: Odometry.h:50
rtabmap::Parameters::filterParameters
static ParametersMap filterParameters(const ParametersMap &parameters, const std::string &group, bool remove=false)
Definition: Parameters.cpp:217
matches
matches
UFATAL
#define UFATAL(...)
data
int data[]
glm::sqrt
GLM_FUNC_DECL vecType< T, P > sqrt(vecType< T, P > const &x)
rtabmap::Odometry::reset
virtual void reset(const Transform &initialPose=Transform::getIdentity())
Definition: Odometry.cpp:210
info
else if n * info
params
SmartProjectionParams params(gtsam::HESSIAN, gtsam::ZERO_ON_DEGENERACY)
rtabmap::Parameters
Definition: Parameters.h:170
rtabmap::Transform::setIdentity
void setIdentity()
Definition: Transform.cpp:157
rtabmap::OdometryViso2::computeTransform
virtual Transform computeTransform(SensorData &image, const Transform &guess=Transform(), OdometryInfo *info=0)
Definition: OdometryViso2.cpp:95
rtabmap::Transform::inverse
Transform inverse() const
Definition: Transform.cpp:178
rtabmap::OdometryViso2::OdometryViso2
OdometryViso2(const rtabmap::ParametersMap &parameters=rtabmap::ParametersMap())
Definition: OdometryViso2.cpp:54
rtabmap::OdometryViso2::~OdometryViso2
virtual ~OdometryViso2()
Definition: OdometryViso2.cpp:72
timer::elapsed
double elapsed() const
ULogger.h
ULogger class and convenient macros.
rtabmap::OdometryViso2::previousLocalTransform_
Transform previousLocalTransform_
Definition: OdometryViso2.h:59
rtabmap::Transform
Definition: Transform.h:41
util2d.h
rtabmap::Transform::isIdentity
bool isIdentity() const
Definition: Transform.cpp:136
UStl.h
Wrappers of STL for convenient functions.
rtabmap::Odometry
Definition: Odometry.h:42
UTimer
Definition: UTimer.h:46
rtabmap::OdometryInfo
Definition: OdometryInfo.h:40
Eigen.Matrix< double, Eigen::Dynamic, Eigen::Dynamic, Eigen::RowMajor >
false
#define false
Definition: ConvertUTF.c:56
t
Point2 t(10, 10)
rtabmap
Definition: CameraARCore.cpp:35
UERROR
#define UERROR(...)
i
int i
int32_t
::int32_t int32_t
rtabmap::Odometry::isInfoDataFilled
bool isInfoDataFilled() const
Definition: Odometry.h:77


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