OdometryOpen3D.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/odometry/OdometryOpen3D.h"
29 #include "rtabmap/core/OdometryInfo.h"
30 #include "rtabmap/core/util2d.h"
31 #include "rtabmap/utilite/ULogger.h"
32 #include "rtabmap/utilite/UTimer.h"
33 
34 #ifdef RTABMAP_OPEN3D
35 #include <open3d/pipelines/odometry/Odometry.h>
36 #include <open3d/geometry/RGBDImage.h>
37 #include <open3d/t/pipelines/odometry/RGBDOdometry.h>
38 #endif
39 
40 namespace rtabmap {
41 
46 OdometryOpen3D::OdometryOpen3D(const ParametersMap & parameters) :
47  Odometry(parameters)
48 #ifdef RTABMAP_OPEN3D
49  ,method_(Parameters::defaultOdomOpen3DMethod()),
50  maxDepth_(Parameters::defaultOdomOpen3DMaxDepth()),
51  keyFrameThr_(Parameters::defaultOdomKeyFrameThr())
52 #endif
53 {
54 #ifdef RTABMAP_OPEN3D
55  Parameters::parse(parameters, Parameters::kOdomOpen3DMethod(), method_);
56  UASSERT(method_>=0 && method_<=2);
57  Parameters::parse(parameters, Parameters::kOdomOpen3DMaxDepth(), maxDepth_);
58  Parameters::parse(parameters, Parameters::kOdomKeyFrameThr(), keyFrameThr_);
59 #endif
60 }
61 
62 OdometryOpen3D::~OdometryOpen3D()
63 {
64 }
65 
66 void OdometryOpen3D::reset(const Transform & initialPose)
67 {
68  Odometry::reset(initialPose);
69 #ifdef RTABMAP_OPEN3D
70  keyFrame_ = SensorData();
71  lastKeyFramePose_.setNull();
72 #endif
73 }
74 
75 #ifdef RTABMAP_OPEN3D
76 open3d::geometry::Image toOpen3D(const cv::Mat & image)
77 {
78  if(image.type() == CV_16UC1)
79  {
80  // convert to float
81  return toOpen3D(util2d::cvtDepthToFloat(image));
82  }
83  open3d::geometry::Image output;
84  output.width_ = image.cols;
85  output.height_ = image.rows;
86  output.num_of_channels_ = image.channels();
87  output.bytes_per_channel_ = image.elemSize()/image.channels();
88  output.data_.resize(image.total()*image.elemSize());
89  memcpy(output.data_.data(), image.data, output.data_.size());
90  return output;
91 }
92 
93 open3d::geometry::RGBDImage toOpen3D(const SensorData & data)
94 {
95  return open3d::geometry::RGBDImage(
96  toOpen3D(data.imageRaw()),
97  toOpen3D(data.depthRaw()));
98 }
99 
100 open3d::camera::PinholeCameraIntrinsic toOpen3D(const CameraModel & model)
101 {
102  return open3d::camera::PinholeCameraIntrinsic(
103  model.imageWidth(),
104  model.imageHeight(),
105  model.fx(),
106  model.fy(),
107  model.cx(),
108  model.cy());
109 }
110 //Tensor versions
111 open3d::t::geometry::Image toOpen3Dt(const cv::Mat & image)
112 {
113  if(image.type() == CV_16UC1)
114  {
115  // convert to float
116  return toOpen3Dt(util2d::cvtDepthToFloat(image));
117  }
118 
119  if(image.type()==CV_32FC1)
120  {
121  return open3d::core::Tensor(
122  (const float_t*)image.data,
123  {image.rows, image.cols, image.channels()},
124  open3d::core::Float32);
125  }
126  else
127  {
128  return open3d::core::Tensor(
129  static_cast<const uint8_t*>(image.data),
130  {image.rows, image.cols, image.channels()},
131  open3d::core::UInt8);
132  }
133 }
134 
135 open3d::t::geometry::RGBDImage toOpen3Dt(const SensorData & data)
136 {
137  return open3d::t::geometry::RGBDImage(
138  toOpen3Dt(data.imageRaw()),
139  toOpen3Dt(data.depthRaw()));
140 }
141 
142 open3d::core::Tensor toOpen3Dt(const CameraModel & model)
143 {
144  return open3d::core::Tensor::Init<double>(
145  {{model.fx(), 0, model.cx()},
146  {0, model.fy(), model.cy()},
147  {0, 0, 1}});
148 }
149 
150 open3d::core::Tensor toOpen3Dt(const Transform & t)
151 {
152  return open3d::core::Tensor::Init<double>(
153  {{t.r11(), t.r12(), t.r13(), t.x()},
154  {t.r21(), t.r22(), t.r23(), t.y()},
155  {t.r31(), t.r32(), t.r33(), t.z()},
156  {0,0,0,1}});
157 }
158 #endif
159 
160 // return not null transform if odometry is correctly computed
161 Transform OdometryOpen3D::computeTransform(
162  SensorData & data,
163  const Transform & guess,
164  OdometryInfo * info)
165 {
166  Transform t;
167 #ifdef RTABMAP_OPEN3D
168  UTimer timer;
169 
170  if(data.imageRaw().empty() || data.depthRaw().empty() || data.cameraModels().size()!=1)
171  {
172  UERROR("Open3D works only with single RGB-D data. Aborting odometry update...");
173  return t;
174  }
175 
176  cv::Mat covariance = cv::Mat::eye(6,6,CV_64FC1)*9999;
177 
178  bool updateKeyFrame = false;
179  if(lastKeyFramePose_.isNull())
180  {
181  lastKeyFramePose_ = this->getPose(); // reset to current pose
182  }
183  Transform motionSinceLastKeyFrame = lastKeyFramePose_.inverse()*this->getPose();
184 
185  if(keyFrame_.isValid())
186  {
187  /*bool tensor = true;
188  if(!tensor)
189  {
190  // Approach in open3d/pipelines/odometry
191  open3d::geometry::RGBDImage source = toOpen3D(data);
192  open3d::geometry::RGBDImage target = toOpen3D(keyFrame_);
193  open3d::camera::PinholeCameraIntrinsic intrinsics = toOpen3D(data.cameraModels()[0]);
194  UDEBUG("Data conversion to Open3D format: %fs", timer.ticks());
195  open3d::pipelines::odometry::OdometryOption option;
196  std::tuple<bool, Eigen::Matrix4d, Eigen::Matrix6d> ret = open3d::pipelines::odometry::ComputeRGBDOdometry(
197  source,
198  target,
199  intrinsics,
200  Eigen::Matrix4d::Identity(),
201  open3d::pipelines::odometry::RGBDOdometryJacobianFromHybridTerm(),
202  option);
203  UDEBUG("Compute Open3D odometry: %fs", timer.ticks());
204  if(std::get<0>(ret))
205  {
206  t = Transform::fromEigen4d(std::get<1>(ret));
207  // from camera frame to base frame
208  t = data.cameraModels()[0].localTransform() * t * data.cameraModels()[0].localTransform().inverse();
209  covariance = cv::Mat::eye(6,6,CV_64FC1)/100;
210  }
211  else
212  {
213  UWARN("Open3D odometry update failed!");
214  }
215  }
216  else*/
217  {
218  // Approach in open3d/t/pipelines/odometry
219  open3d::t::geometry::RGBDImage source = toOpen3Dt(data);
220  open3d::t::geometry::RGBDImage target = toOpen3Dt(keyFrame_);
221  open3d::core::Tensor intrinsics = toOpen3Dt(data.cameraModels()[0]);
222  Transform baseToCamera = data.cameraModels()[0].localTransform();
223  open3d::core::Tensor odomInit;
224  if(guess.isNull())
225  {
226  odomInit = open3d::core::Tensor::Eye(4, open3d::core::Float64, open3d::core::Device("CPU:0"));
227  }
228  else
229  {
230  odomInit = toOpen3Dt(baseToCamera.inverse() * (motionSinceLastKeyFrame*guess) * baseToCamera);
231  }
232  UDEBUG("Data conversion to Open3D format: %fs", timer.ticks());
233  open3d::t::pipelines::odometry::OdometryResult ret = open3d::t::pipelines::odometry::RGBDOdometryMultiScale(
234  source,
235  target,
236  intrinsics,
237  odomInit,
238  1.0f,
239  maxDepth_,
240  {10, 5, 3},
241  (open3d::t::pipelines::odometry::Method)method_,
242  open3d::t::pipelines::odometry::OdometryLossParams());
243  UDEBUG("Compute Open3D odometry: %fs", timer.ticks());
244  if(ret.fitness_!=0)
245  {
246  const double * ptr = (const double *)ret.transformation_.GetDataPtr();
247  t = Transform(
248  ptr[0], ptr[1], ptr[2], ptr[3],
249  ptr[4], ptr[5], ptr[6], ptr[7],
250  ptr[8], ptr[9], ptr[10],ptr[11]);
251  // from camera frame to base frame
252  t = baseToCamera * t * baseToCamera.inverse();
253 
254  t = motionSinceLastKeyFrame.inverse() * t;
255 
256  //based on values set in viso2_ros
257  covariance = cv::Mat::eye(6,6, CV_64FC1);
258  covariance.at<double>(0,0) = 0.002;
259  covariance.at<double>(1,1) = 0.002;
260  covariance.at<double>(2,2) = 0.05;
261  covariance.at<double>(3,3) = 0.09;
262  covariance.at<double>(4,4) = 0.09;
263  covariance.at<double>(5,5) = 0.09;
264 
265  if(info)
266  {
267  info->reg.icpRMS = ret.inlier_rmse_;
268  info->reg.icpInliersRatio = ret.fitness_;
269  }
270 
271  if(ret.fitness_ < keyFrameThr_)
272  {
273  updateKeyFrame = true;
274  }
275  }
276  else
277  {
278  UWARN("Open3D odometry update failed!");
279  }
280  }
281  }
282  else
283  {
284  t.setIdentity();
285  updateKeyFrame = true;
286  }
287 
288  if(updateKeyFrame)
289  {
290  keyFrame_ = data;
291  lastKeyFramePose_.setNull();
292  }
293 
294  if(info)
295  {
296  info->reg.covariance = covariance;
297  info->keyFrameAdded = updateKeyFrame;
298  }
299 
300 #else
301  UERROR("RTAB-Map is not built with Open3D support! Select another odometry approach.");
302 #endif
303  return t;
304 }
305 
306 } // namespace rtabmap
rtabmap::SensorData
Definition: SensorData.h:51
OdometryInfo.h
rtabmap::OdometryOpen3D::~OdometryOpen3D
virtual ~OdometryOpen3D()
Definition: OdometryOpen3D.cpp:62
timer
ret
int ret
rtabmap::OdometryOpen3D::computeTransform
virtual Transform computeTransform(SensorData &image, const Transform &guess=Transform(), OdometryInfo *info=0)
Definition: OdometryOpen3D.cpp:161
rtabmap::util2d::cvtDepthToFloat
cv::Mat RTABMAP_CORE_EXPORT cvtDepthToFloat(const cv::Mat &depth16U)
Definition: util2d.cpp:928
rtabmap::ParametersMap
std::map< std::string, std::string > ParametersMap
Definition: Parameters.h:43
Vector2::y
float y
UTimer.h
OdometryOpen3D.h
rtabmap::Transform::isNull
bool isNull() const
Definition: Transform.cpp:107
rtabmap::OdometryOpen3D::OdometryOpen3D
OdometryOpen3D(const rtabmap::ParametersMap &parameters=rtabmap::ParametersMap())
Definition: OdometryOpen3D.cpp:46
rtabmap::Parameters::parse
static bool parse(const ParametersMap &parameters, const std::string &key, bool &value)
Definition: Parameters.cpp:500
rtabmap::Odometry::getPose
const Transform & getPose() const
Definition: Odometry.h:76
data
int data[]
source
const char * source
Definition: lz4hc.h:181
rtabmap::OdometryOpen3D::reset
virtual void reset(const Transform &initialPose=Transform::getIdentity())
Definition: OdometryOpen3D.cpp:66
rtabmap::Odometry::reset
virtual void reset(const Transform &initialPose=Transform::getIdentity())
Definition: Odometry.cpp:210
info
else if n * info
target
target
UASSERT
#define UASSERT(condition)
rtabmap::Parameters
Definition: Parameters.h:170
rtabmap::Transform::inverse
Transform inverse() const
Definition: Transform.cpp:178
UWARN
#define UWARN(...)
f
Point2(* f)(const Point3 &, OptionalJacobian< 2, 3 >)
ULogger.h
ULogger class and convenient macros.
rtabmap::Transform
Definition: Transform.h:41
util2d.h
uint8_t
::uint8_t uint8_t
UDEBUG
#define UDEBUG(...)
rtabmap::Odometry
Definition: Odometry.h:42
UTimer
Definition: UTimer.h:46
rtabmap::OdometryInfo
Definition: OdometryInfo.h:40
Vector2::x
float x
t
Point2 t(10, 10)
rtabmap
Definition: CameraARCore.cpp:35
glm::float_t
mediump_float float_t
Definition: type_float.hpp:69
UERROR
#define UERROR(...)
trace.model
model
Definition: trace.py:4

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