34 #include <opencv2/imgproc/types_c.h> 37 #include <dvo/dense_tracking.h> 38 #include <dvo/core/surface_pyramid.h> 39 #include <dvo/core/rgbd_image.h> 52 motionFromKeyFrame_(
Transform::getIdentity())
105 UERROR(
"Not supported input!");
111 UERROR(
"Invalid camera model! Only single RGB-D camera supported by DVO. Try another odometry approach.");
117 dvo::DenseTracker::Config cfg = dvo::DenseTracker::getDefaultConfig();
119 dvo_ =
new dvo::DenseTracker(cfg);
122 cv::Mat grey, grey_s16, depth_inpainted, depth_mask, depth_mono, depth_float;
123 if(data.
imageRaw().type() != CV_32FC1)
125 if(data.
imageRaw().type() == CV_8UC3)
127 cv::cvtColor(data.
imageRaw(), grey, CV_BGR2GRAY);
134 grey.convertTo(grey_s16, CV_32F);
142 if(data.
depthRaw().type() == CV_32FC1)
145 for(
int i=0; i<depth_float.rows; ++i)
147 for(
int j=0; j<depth_float.cols; ++j)
149 float &
d = depth_float.at<
float>(i,j);
157 else if(data.
depthRaw().type() == CV_16UC1)
159 depth_float = cv::Mat(data.
depthRaw().size(), CV_32FC1);
160 for(
int i=0; i<data.
depthRaw().rows; ++i)
162 for(
int j=0; j<data.
depthRaw().cols; ++j)
164 float d = float(data.
depthRaw().at<
unsigned short>(i,j))/1000.0f;
165 depth_float.at<
float>(i, j) = d==0.0
f?NAN:d;
171 UFATAL(
"Unknown depth format!");
176 dvo::core::IntrinsicMatrix intrinsics = dvo::core::IntrinsicMatrix::create(
181 camera_ =
new dvo::core::RgbdCameraPyramid(
187 dvo::core::RgbdImagePyramid * current =
new dvo::core::RgbdImagePyramid(*camera_, grey_s16, depth_float);
193 reference_ = current;
198 covariance = cv::Mat::eye(6,6,CV_64FC1) * 9999.0;
202 dvo::DenseTracker::Result result;
203 dvo_->match(*reference_, *current, result);
206 if(result.Information(0,0) > 0.0 && result.Information(0,0) != 1.0)
209 cv::Mat information = cv::Mat::eye(6,6, CV_64FC1);
210 memcpy(information.data, result.Information.data(), 36*
sizeof(double));
212 covariance = cv::Mat::eye(6,6,CV_64FC1);
213 covariance = information.inv().mul(covariance);
228 reference_ = current;
246 covariance = cv::Mat::eye(6,6,CV_64FC1) * 9999.0;
247 UWARN(
"dvo failed to estimate motion, tracking will be reinitialized on next frame.");
259 t = localTransform * t * localTransform.
inverse();
274 UERROR(
"RTAB-Map is not built with DVO support! Select another visual odometry approach.");
virtual void reset(const Transform &initialPose=Transform::getIdentity())
virtual Transform computeTransform(SensorData &image, const Transform &guess=Transform(), OdometryInfo *info=0)
std::map< std::string, std::string > ParametersMap
const cv::Mat & imageRaw() const
Wrappers of STL for convenient functions.
OdometryDVO(const rtabmap::ParametersMap ¶meters=rtabmap::ParametersMap())
Transform previousLocalTransform_
const cv::Mat & depthOrRightRaw() const
Transform motionFromKeyFrame_
const std::vector< CameraModel > & cameraModels() const
ULogger class and convenient macros.
virtual void reset(const Transform &initialPose=Transform::getIdentity())