36 #include <opencv2/imgproc/types_c.h>
39 #include <estimator/estimator.h>
40 #include <estimator/parameters.h>
41 #include <camodocal/camera_models/PinholeCamera.h>
42 #include <camodocal/camera_models/EquidistantCamera.h>
43 #include <utility/visualization.h>
49 class VinsEstimator:
public Estimator
53 const Transform & imuLocalTransform,
54 const StereoCameraModel &
model,
55 bool rectified) : Estimator()
61 UASSERT(NUM_OF_CAM >= 1 && NUM_OF_CAM <=2);
63 if( (NUM_OF_CAM == 2 &&
model.left().D_raw().cols == 4 &&
model.right().D_raw().cols == 4) ||
64 (NUM_OF_CAM == 1 &&
model.left().D_raw().cols == 4))
66 UWARN(
"Overwriting VINS camera calibration config with received pinhole model... rectified=%d", rectified?1:0);
67 featureTracker.m_camera.clear();
69 camodocal::PinholeCameraPtr
camera(
new camodocal::PinholeCamera );
70 camodocal::PinholeCamera::Parameters
params(
72 model.left().imageWidth(),
model.left().imageHeight(),
73 rectified?0:
model.left().D_raw().at<
double>(0,0),
74 rectified?0:
model.left().D_raw().at<
double>(0,1),
75 rectified?0:
model.left().D_raw().at<
double>(0,2),
76 rectified?0:
model.left().D_raw().at<
double>(0,3),
77 rectified?
model.left().fx():
model.left().K_raw().at<
double>(0,0),
78 rectified?
model.left().fy():
model.left().K_raw().at<
double>(1,1),
79 rectified?
model.left().cx():
model.left().K_raw().at<
double>(0,2),
80 rectified?
model.left().cy():
model.left().K_raw().at<
double>(1,2));
81 camera->setParameters(params);
82 featureTracker.m_camera.push_back(camera);
86 camodocal::PinholeCameraPtr
camera(
new camodocal::PinholeCamera );
87 camodocal::PinholeCamera::Parameters
params(
89 model.right().imageWidth(),
model.right().imageHeight(),
90 rectified?0:
model.right().D_raw().at<
double>(0,0),
91 rectified?0:
model.right().D_raw().at<
double>(0,1),
92 rectified?0:
model.right().D_raw().at<
double>(0,2),
93 rectified?0:
model.right().D_raw().at<
double>(0,3),
94 rectified?
model.right().fx():
model.right().K_raw().at<
double>(0,0),
95 rectified?
model.right().fy():
model.right().K_raw().at<
double>(1,1),
96 rectified?
model.right().cx():
model.right().K_raw().at<
double>(0,2),
97 rectified?
model.right().cy():
model.right().K_raw().at<
double>(1,2));
98 camera->setParameters(params);
99 featureTracker.m_camera.push_back(camera);
104 UWARN(
"Images are rectified but received calibration cannot be "
105 "used, make sure calibration in config file doesn't have "
106 "distortion or send raw images to VINS odometry.");
107 if(!featureTracker.m_camera.empty())
109 if(featureTracker.m_camera.front()->imageWidth() !=
model.left().imageWidth() ||
110 featureTracker.m_camera.front()->imageHeight() !=
model.left().imageHeight())
112 UERROR(
"Received images don't have same size (%dx%d) than in the config file (%dx%d)!",
113 model.left().imageWidth(),
114 model.left().imageHeight(),
115 featureTracker.m_camera.front()->imageWidth(),
116 featureTracker.m_camera.front()->imageHeight());
121 Transform imuCam0 = imuLocalTransform.inverse() *
model.localTransform();
123 tic[0] = Vector3d(imuCam0.x(), imuCam0.y(), imuCam0.z());
124 ric[0] = imuCam0.toEigen4d().block<3,3>(0,0);
131 cam0cam1 = Transform(
132 1, 0, 0,
model.baseline(),
138 cam0cam1 =
model.stereoTransform().inverse();
141 Transform imuCam1 = imuCam0 * cam0cam1;
143 tic[1] = Vector3d(imuCam1.x(), imuCam1.y(), imuCam1.z());
144 ric[1] = imuCam1.toEigen4d().block<3,3>(0,0);
147 for (
int i = 0;
i < NUM_OF_CAM;
i++)
149 cout <<
" exitrinsic cam " <<
i << endl << ric[
i] << endl <<
tic[
i].transpose() << endl;
151 f_manager.setRic(ric);
152 ProjectionTwoFrameOneCamFactor::sqrt_info = FOCAL_LENGTH / 1.5 * Matrix2d::Identity();
153 ProjectionTwoFrameTwoCamFactor::sqrt_info = FOCAL_LENGTH / 1.5 * Matrix2d::Identity();
154 ProjectionOneFrameTwoCamFactor::sqrt_info = FOCAL_LENGTH / 1.5 * Matrix2d::Identity();
157 cout <<
"set g " <<
g.transpose() << endl;
161 void inputImage(
double t,
const cv::Mat &_img,
const cv::Mat &_img1)
164 map<int, vector<pair<int, Eigen::Matrix<double, 7, 1>>>> featureFrame;
165 TicToc featureTrackerTime;
167 featureFrame = featureTracker.trackImage(t, _img);
169 featureFrame = featureTracker.trackImage(t, _img, _img1);
184 featureBuf.push(make_pair(t, featureFrame));
187 processMeasurements();
188 UDEBUG(
"VINS process time: %f", processTime.toc());
194 void inputIMU(
double t,
const Vector3d &linearAcceleration,
const Vector3d &angularVelocity)
197 accBuf.push(make_pair(t, linearAcceleration));
198 gyrBuf.push(make_pair(t, angularVelocity));
202 fastPredictIMU(t, linearAcceleration, angularVelocity);
208 void processMeasurements()
213 pair<double, map<int, vector<pair<int, Eigen::Matrix<double, 7, 1> > > > > feature;
214 vector<pair<double, Eigen::Vector3d>> accVector, gyrVector;
215 if(!featureBuf.empty())
217 feature = featureBuf.front();
218 curTime = feature.first + td;
221 if (!((!USE_IMU || IMUAvailable(feature.first + td))))
226 printf(
"wait for imu ... \n");
235 getIMUInterval(prevTime, curTime, accVector, gyrVector);
242 if(!initFirstPoseFlag)
243 initFirstIMUPose(accVector);
244 UDEBUG(
"accVector.size() = %d", accVector.size());
245 for(
size_t i = 0;
i < accVector.size();
i++)
249 dt = accVector[
i].first - prevTime;
250 else if (i == accVector.size() - 1)
251 dt = curTime - accVector[
i - 1].first;
253 dt = accVector[
i].first - accVector[
i - 1].first;
254 processIMU(accVector[i].first, dt, accVector[i].second, gyrVector[i].second);
258 processImage(feature.second, feature.first);
262 printStatistics(*
this, 0);
297 std::string configFilename;
298 Parameters::parse(parameters, Parameters::kOdomVINSConfigPath(), configFilename);
299 if(configFilename.empty())
301 UERROR(
"VINS config file is empty (%s=%s)!",
302 Parameters::kOdomVINSConfigPath().
c_str(),
303 Parameters::kOdomVINSConfigPath().
c_str());
315 delete vinsEstimator_;
325 delete vinsEstimator_;
327 previousPose_.setIdentity();
329 previousLocalTransform_.setNull();
331 initGravity_ =
false;
345 if(USE_IMU!=0 && !
data.imu().empty())
347 double t =
data.stamp();
348 double dx =
data.imu().linearAcceleration().val[0];
349 double dy =
data.imu().linearAcceleration().val[1];
350 double dz =
data.imu().linearAcceleration().val[2];
351 double rx =
data.imu().angularVelocity().val[0];
352 double ry =
data.imu().angularVelocity().val[1];
353 double rz =
data.imu().angularVelocity().val[2];
354 Vector3d acc(dx, dy, dz);
355 Vector3d gyr(rx, ry, rz);
359 if(vinsEstimator_ != 0)
361 vinsEstimator_->inputIMU(
t, acc, gyr);
365 lastImu_ =
data.imu();
366 UWARN(
"Waiting an image for initialization...");
370 if(!
data.imageRaw().empty() && !
data.rightRaw().empty() &&
data.stereoCameraModels().size() == 1 &&
data.stereoCameraModels()[0].isValidForProjection())
372 if(USE_IMU==1 && lastImu_.localTransform().isNull())
374 UWARN(
"Waiting IMU for initialization...");
377 if(vinsEstimator_ == 0)
380 vinsEstimator_ =
new VinsEstimator(
382 data.stereoCameraModels()[0],
383 this->imagesAlreadyRectified());
386 UDEBUG(
"Image update stamp=%f",
data.stamp());
389 if(
data.imageRaw().type() == CV_8UC3)
391 cv::cvtColor(
data.imageRaw(), left, CV_BGR2GRAY);
393 else if(
data.imageRaw().type() == CV_8UC1)
395 left =
data.imageRaw().clone();
399 UFATAL(
"Not supported color type!");
401 if(
data.rightRaw().type() == CV_8UC3)
403 cv::cvtColor(
data.rightRaw(), right, CV_BGR2GRAY);
405 else if(
data.rightRaw().type() == CV_8UC1)
407 right =
data.rightRaw().clone();
411 UFATAL(
"Not supported color type!");
414 vinsEstimator_->inputImage(
data.stamp(), left, right);
416 if(vinsEstimator_->solver_flag == Estimator::NON_LINEAR)
419 tmp_Q =
Quaterniond(vinsEstimator_->Rs[WINDOW_SIZE]);
421 vinsEstimator_->Ps[WINDOW_SIZE].x(),
422 vinsEstimator_->Ps[WINDOW_SIZE].y(),
423 vinsEstimator_->Ps[WINDOW_SIZE].z(),
431 if(!lastImu_.localTransform().isNull())
433 p =
p * lastImu_.localTransform().inverse();
442 if(previousPose_.isIdentity())
449 t = previousPoseInv*
p;
460 for (
auto &it_per_id : vinsEstimator_->f_manager.feature)
463 used_num = it_per_id.feature_per_frame.
size();
464 if (!(used_num >= 2 && it_per_id.start_frame < WINDOW_SIZE - 2))
466 if (it_per_id.start_frame > WINDOW_SIZE * 3.0 / 4.0 || it_per_id.solve_flag != 1)
468 int imu_i = it_per_id.start_frame;
469 Vector3d pts_i = it_per_id.feature_per_frame[it_per_id.feature_per_frame.size()-1].point * it_per_id.estimated_depth;
470 Vector3d w_pts_i = vinsEstimator_->Rs[imu_i] * (vinsEstimator_->ric[0] * pts_i + vinsEstimator_->tic[0]) + vinsEstimator_->Ps[imu_i];
477 info->localMap.insert(std::make_pair(it_per_id.feature_id,
p));
482 data.stereoCameraModels()[0].left().reproject(pts_i(0), pts_i(1), pts_i(2), pt.x, pt.y);
483 info->reg.inliersIDs.push_back(
info->newCorners.size());
484 info->newCorners.push_back(pt);
487 info->features =
info->newCorners.size();
488 info->localMapSize =
info->localMap.size();
495 UWARN(
"VINS not yet initialized... waiting to get enough IMU messages");
498 else if(!
data.imageRaw().empty() && !
data.depthRaw().empty())
500 UERROR(
"VINS-Fusion doesn't work with RGB-D data, stereo images are required!");
502 else if(!
data.imageRaw().empty() &&
data.depthOrRightRaw().empty())
504 UERROR(
"VINS-Fusion requires stereo images!");
506 else if(
data.imu().empty())
508 UERROR(
"VINS-Fusion requires stereo images (and only one stereo camera with valid calibration)!");
511 UERROR(
"RTAB-Map is not built with VINS support! Select another visual odometry approach.");