35 #include <open3d/pipelines/odometry/Odometry.h>
36 #include <open3d/geometry/RGBDImage.h>
37 #include <open3d/t/pipelines/odometry/RGBDOdometry.h>
49 ,method_(
Parameters::defaultOdomOpen3DMethod()),
50 maxDepth_(
Parameters::defaultOdomOpen3DMaxDepth()),
51 keyFrameThr_(
Parameters::defaultOdomKeyFrameThr())
56 UASSERT(method_>=0 && method_<=2);
71 lastKeyFramePose_.setNull();
76 open3d::geometry::Image toOpen3D(
const cv::Mat & image)
78 if(image.type() == CV_16UC1)
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());
93 open3d::geometry::RGBDImage toOpen3D(
const SensorData & data)
95 return open3d::geometry::RGBDImage(
96 toOpen3D(
data.imageRaw()),
97 toOpen3D(
data.depthRaw()));
100 open3d::camera::PinholeCameraIntrinsic toOpen3D(
const CameraModel &
model)
102 return open3d::camera::PinholeCameraIntrinsic(
111 open3d::t::geometry::Image toOpen3Dt(
const cv::Mat & image)
113 if(image.type() == CV_16UC1)
119 if(image.type()==CV_32FC1)
121 return open3d::core::Tensor(
123 {image.rows, image.cols, image.channels()},
124 open3d::core::Float32);
128 return open3d::core::Tensor(
129 static_cast<const uint8_t*
>(image.data),
130 {image.rows, image.cols, image.channels()},
131 open3d::core::UInt8);
135 open3d::t::geometry::RGBDImage toOpen3Dt(
const SensorData & data)
137 return open3d::t::geometry::RGBDImage(
138 toOpen3Dt(
data.imageRaw()),
139 toOpen3Dt(
data.depthRaw()));
142 open3d::core::Tensor toOpen3Dt(
const CameraModel &
model)
144 return open3d::core::Tensor::Init<double>(
150 open3d::core::Tensor toOpen3Dt(
const Transform & t)
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()},
167 #ifdef RTABMAP_OPEN3D
170 if(
data.imageRaw().empty() ||
data.depthRaw().empty() ||
data.cameraModels().size()!=1)
172 UERROR(
"Open3D works only with single RGB-D data. Aborting odometry update...");
176 cv::Mat covariance = cv::Mat::eye(6,6,CV_64FC1)*9999;
178 bool updateKeyFrame =
false;
179 if(lastKeyFramePose_.isNull())
181 lastKeyFramePose_ = this->
getPose();
185 if(keyFrame_.isValid())
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;
226 odomInit = open3d::core::Tensor::Eye(4, open3d::core::Float64, open3d::core::Device(
"CPU:0"));
230 odomInit = toOpen3Dt(baseToCamera.
inverse() * (motionSinceLastKeyFrame*guess) * baseToCamera);
232 UDEBUG(
"Data conversion to Open3D format: %fs",
timer.ticks());
233 open3d::t::pipelines::odometry::OdometryResult
ret = open3d::t::pipelines::odometry::RGBDOdometryMultiScale(
241 (open3d::t::pipelines::odometry::Method)method_,
242 open3d::t::pipelines::odometry::OdometryLossParams());
243 UDEBUG(
"Compute Open3D odometry: %fs",
timer.ticks());
246 const double * ptr = (
const double *)
ret.transformation_.GetDataPtr();
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]);
252 t = baseToCamera *
t * baseToCamera.
inverse();
254 t = motionSinceLastKeyFrame.
inverse() *
t;
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;
267 info->reg.icpRMS =
ret.inlier_rmse_;
268 info->reg.icpInliersRatio =
ret.fitness_;
271 if(
ret.fitness_ < keyFrameThr_)
273 updateKeyFrame =
true;
278 UWARN(
"Open3D odometry update failed!");
285 updateKeyFrame =
true;
291 lastKeyFramePose_.setNull();
296 info->reg.covariance = covariance;
297 info->keyFrameAdded = updateKeyFrame;
301 UERROR(
"RTAB-Map is not built with Open3D support! Select another odometry approach.");