i/rtabmap
1
0
Fork 0
mirror of https://github.com/introlab/rtabmap.git synced 2026-09-02 17:40:23 +08:00
Code Issues Packages Projects Releases Wiki Activity

VINS Fusion overhaul (#1580)

Browse Source 
* Updated correct focal length used by VINS, updated VINS visualization.

* Renamed OdometryVINS to OdometryVINSFusion
This commit is contained in:
matlabbe
2025-09-14 19:29:28 -07:00
committed by GitHub
parent 6a435c968f
commit cb0cc9ed18
15 changed files with 245 additions and 211 deletions
Download Patch File Download Diff File Expand all files Collapse all files

2
corelib/src/CMakeLists.txt
View File

@@ -98,7 +98,7 @@ SET(SRC_FILES
odometry/OdometryLOAM.cpp
odometry/OdometryFLOAM.cpp
odometry/OdometryMSCKF.cpp
odometry/OdometryVINS.cpp
odometry/OdometryVINSFusion.cpp
odometry/OdometryOpenVINS.cpp
odometry/OdometryOpen3D.cpp

6
corelib/src/Odometry.cpp
View File

@@ -36,7 +36,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
#include "rtabmap/core/odometry/OdometryLOAM.h"
#include "rtabmap/core/odometry/OdometryFLOAM.h"
#include "rtabmap/core/odometry/OdometryMSCKF.h"
#include "rtabmap/core/odometry/OdometryVINS.h"
#include "rtabmap/core/odometry/OdometryVINSFusion.h"
#include "rtabmap/core/odometry/OdometryOpenVINS.h"
#include "rtabmap/core/odometry/OdometryOpen3D.h"
#include "rtabmap/core/OdometryInfo.h"
@@ -103,8 +103,8 @@ Odometry * Odometry::create(Odometry::Type & type, const ParametersMap & paramet
case Odometry::kTypeMSCKF:
odometry = new OdometryMSCKF(parameters);
break;
case Odometry::kTypeVINS:
odometry = new OdometryVINS(parameters);
case Odometry::kTypeVINSFusion:
odometry = new OdometryVINSFusion(parameters);
break;
case Odometry::kTypeOpenVINS:
odometry = new OdometryOpenVINS(parameters);

5
corelib/src/Parameters.cpp
View File

@@ -236,6 +236,9 @@ const std::map<std::string, std::pair<bool, std::string> > & Parameters::getRemo
{
// removed parameters
// 0.23.1
removedParameters_.insert(std::make_pair("OdomVINS/ConfigPath", std::make_pair(true, Parameters::kOdomVINSFusionConfigPath())));
// 0.21.13
removedParameters_.insert(std::make_pair("Vis/ForwardEstOnly", std::make_pair(false, "")));
@@ -936,7 +939,7 @@ ParametersMap Parameters::parseArguments(int argc, char * argv[], bool onlyParam
std::cout << str << std::setw(spacing - str.size()) << "false" << std::endl;
#endif
str = "With VINS-Fusion:";
#ifdef RTABMAP_VINS
#ifdef RTABMAP_VINS_FUSION
std::cout << str << std::setw(spacing - str.size()) << "true" << std::endl;
#else
std::cout << str << std::setw(spacing - str.size()) << "false" << std::endl;

282
corelib/src/odometry/OdometryVINS.cpp → corelib/src/odometry/OdometryVINSFusion.cpp
View File

@@ -25,7 +25,7 @@ ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
*/
#include "rtabmap/core/odometry/OdometryVINS.h"
#include "rtabmap/core/odometry/OdometryVINSFusion.h"
#include "rtabmap/core/OdometryInfo.h"
#include "rtabmap/core/util3d_transforms.h"
#include "rtabmap/utilite/ULogger.h"
@@ -35,7 +35,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
#include "rtabmap/utilite/UDirectory.h"
#include <opencv2/imgproc/types_c.h>
#ifdef RTABMAP_VINS
#ifdef RTABMAP_VINS_FUSION
#include <estimator/estimator.h>
#include <estimator/parameters.h>
#include <camodocal/camera_models/PinholeCamera.h>
@@ -45,11 +45,11 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
namespace rtabmap {
#ifdef RTABMAP_VINS
class VinsEstimator: public Estimator
#ifdef RTABMAP_VINS_FUSION
class VinsFusionEstimator: public Estimator
{
public:
VinsEstimator(
VinsFusionEstimator(
const Transform & imuLocalTransform,
const StereoCameraModel & model,
bool rectified) : Estimator()
@@ -57,6 +57,9 @@ public:
MULTIPLE_THREAD = 0;
setParameter();
ROW=model.left().imageHeight();
COL=model.left().imageWidth();
//overwrite camera calibration only if received model is radtan, otherwise use config
UASSERT(NUM_OF_CAM >= 1 && NUM_OF_CAM <=2);
@@ -81,6 +84,15 @@ public:
camera->setParameters(params);
featureTracker.m_camera.push_back(camera);
double originalParalax = MIN_PARALLAX * FOCAL_LENGTH;
// If you have compiler error about FOCAL_LENGTH being const, make sure to use the following patch:
// https://gist.github.com/matlabbe/795ab37067367dca58bbadd8201d986c#file-vins-fusion_pull136-patch
FOCAL_LENGTH = params.fx();
MIN_PARALLAX = originalParalax / FOCAL_LENGTH;
ProjectionTwoFrameOneCamFactor::sqrt_info = FOCAL_LENGTH / 1.5 * Matrix2d::Identity();
ProjectionTwoFrameTwoCamFactor::sqrt_info = FOCAL_LENGTH / 1.5 * Matrix2d::Identity();
ProjectionOneFrameTwoCamFactor::sqrt_info = FOCAL_LENGTH / 1.5 * Matrix2d::Identity();
if(NUM_OF_CAM == 2)
{
camodocal::PinholeCameraPtr camera( new camodocal::PinholeCamera );
@@ -120,8 +132,8 @@ public:
Transform imuCam0 = imuLocalTransform.inverse() * model.localTransform();
tic[0] = Vector3d(imuCam0.x(), imuCam0.y(), imuCam0.z());
ric[0] = imuCam0.toEigen4d().block<3,3>(0,0);
tic[0] = TIC[0] = Vector3d(imuCam0.x(), imuCam0.y(), imuCam0.z());
ric[0] = RIC[0] = imuCam0.toEigen4d().block<3,3>(0,0);
if(NUM_OF_CAM == 2)
{
@@ -140,54 +152,39 @@ public:
UASSERT(!cam0cam1.isNull());
Transform imuCam1 = imuCam0 * cam0cam1;
tic[1] = Vector3d(imuCam1.x(), imuCam1.y(), imuCam1.z());
ric[1] = imuCam1.toEigen4d().block<3,3>(0,0);
tic[1] = TIC[0] = Vector3d(imuCam1.x(), imuCam1.y(), imuCam1.z());
ric[1] = RIC[0] = imuCam1.toEigen4d().block<3,3>(0,0);
}
for (int i = 0; i < NUM_OF_CAM; i++)
{
cout << " exitrinsic cam " << i << endl << ric[i] << endl << tic[i].transpose() << endl;
cout << " new extrinsic cam " << i << endl << ric[i] << endl << tic[i].transpose() << endl;
}
for (int i = 0; i < NUM_OF_CAM; i++)
{
cout << " new intrinsic cam " << i << endl << featureTracker.m_camera[i]->parametersToString() << endl;
}
f_manager.setRic(ric);
ProjectionTwoFrameOneCamFactor::sqrt_info = FOCAL_LENGTH / 1.5 * Matrix2d::Identity();
ProjectionTwoFrameTwoCamFactor::sqrt_info = FOCAL_LENGTH / 1.5 * Matrix2d::Identity();
ProjectionOneFrameTwoCamFactor::sqrt_info = FOCAL_LENGTH / 1.5 * Matrix2d::Identity();
td = TD;
g = G;
cout << "set g " << g.transpose() << endl;
}
// Copy of original inputImage() so that overridden processMeasurements() is used and threading is disabled.
void inputImage(double t, const cv::Mat &_img, const cv::Mat &_img1)
{
TicToc processTime;
inputImageCnt++;
map<int, vector<pair<int, Eigen::Matrix<double, 7, 1>>>> featureFrame;
TicToc featureTrackerTime;
if(_img1.empty())
if(_img1.empty()) {
featureFrame = featureTracker.trackImage(t, _img);
else
}
else {
featureFrame = featureTracker.trackImage(t, _img, _img1);
//printf("featureTracker time: %f\n", featureTrackerTime.toc());
//if(MULTIPLE_THREAD)
//{
// if(inputImageCnt % 2 == 0)
// {
// mBuf.lock();
// featureBuf.push(make_pair(t, featureFrame));
// mBuf.unlock();
// }
//}
//else
{
mBuf.lock();
featureBuf.push(make_pair(t, featureFrame));
mBuf.unlock();
TicToc processTime;
processMeasurements();
UDEBUG("VINS process time: %f", processTime.toc());
}
}
mBuf.lock();
featureBuf.push(make_pair(t, featureFrame));
mBuf.unlock();
processMeasurements();
UDEBUG("VINS process time: %f", processTime.toc());
}
// Copy of original inputIMU() but with publisher commented
@@ -199,127 +196,98 @@ public:
//printf("input imu with time %f \n", t);
mBuf.unlock();
fastPredictIMU(t, linearAcceleration, angularVelocity);
//if (solver_flag == NON_LINEAR)
// pubLatestOdometry(latest_P, latest_Q, latest_V, t);
if (solver_flag == NON_LINEAR)
{
mPropagate.lock();
fastPredictIMU(t, linearAcceleration, angularVelocity);
mPropagate.unlock();
}
}
// Copy of original processMeasurements() but with publishers commented and threading disabled
void processMeasurements()
{
//while (1)
pair<double, map<int, vector<pair<int, Eigen::Matrix<double, 7, 1> > > > > feature;
vector<pair<double, Eigen::Vector3d>> accVector, gyrVector;
if(!featureBuf.empty())
{
//printf("process measurments\n");
pair<double, map<int, vector<pair<int, Eigen::Matrix<double, 7, 1> > > > > feature;
vector<pair<double, Eigen::Vector3d>> accVector, gyrVector;
if(!featureBuf.empty())
feature = featureBuf.front();
curTime = feature.first + td;
if (USE_IMU && !IMUAvailable(feature.first + td))
{
feature = featureBuf.front();
curTime = feature.first + td;
//while(1)
//{
if (!((!USE_IMU || IMUAvailable(feature.first + td))))
//if ((!USE_IMU || IMUAvailable(feature.first + td)))
// break;
//else
{
printf("wait for imu ... \n");
//if (! MULTIPLE_THREAD)
return;
//std::chrono::milliseconds dura(5);
//std::this_thread::sleep_for(dura);
}
//}
mBuf.lock();
if(USE_IMU)
getIMUInterval(prevTime, curTime, accVector, gyrVector);
featureBuf.pop();
mBuf.unlock();
if(USE_IMU)
{
if(!initFirstPoseFlag)
initFirstIMUPose(accVector);
UDEBUG("accVector.size() = %d", accVector.size());
for(size_t i = 0; i < accVector.size(); i++)
{
double dt;
if(i == 0)
dt = accVector[i].first - prevTime;
else if (i == accVector.size() - 1)
dt = curTime - accVector[i - 1].first;
else
dt = accVector[i].first - accVector[i - 1].first;
processIMU(accVector[i].first, dt, accVector[i].second, gyrVector[i].second);
}
}
processImage(feature.second, feature.first);
prevTime = curTime;
printStatistics(*this, 0);
//std_msgs::Header header;
//header.frame_id = "world";
//header.stamp = ros::Time(feature.first);
//pubOdometry(*this, header);
//pubKeyPoses(*this, header);
//pubCameraPose(*this, header);
//pubPointCloud(*this, header);
//pubKeyframe(*this);
//pubTF(*this, header);
printf("wait for imu ... \n");
return;
}
mBuf.lock();
if(USE_IMU)
getIMUInterval(prevTime, curTime, accVector, gyrVector);
//if (! MULTIPLE_THREAD)
// break;
featureBuf.pop();
mBuf.unlock();
//std::chrono::milliseconds dura(2);
//std::this_thread::sleep_for(dura);
if(USE_IMU)
{
if(!initFirstPoseFlag)
initFirstIMUPose(accVector);
for(size_t i = 0; i < accVector.size(); i++)
{
double dt;
if(i == 0)
dt = accVector[i].first - prevTime;
else if (i == accVector.size() - 1)
dt = curTime - accVector[i - 1].first;
else
dt = accVector[i].first - accVector[i - 1].first;
processIMU(accVector[i].first, dt, accVector[i].second, gyrVector[i].second);
}
}
mProcess.lock();
processImage(feature.second, feature.first);
prevTime = curTime;
mProcess.unlock();
}
}
};
#endif
OdometryVINS::OdometryVINS(const ParametersMap & parameters) :
OdometryVINSFusion::OdometryVINSFusion(const ParametersMap & parameters) :
Odometry(parameters)
#ifdef RTABMAP_VINS
#ifdef RTABMAP_VINS_FUSION
,
vinsEstimator_(0),
initGravity_(false),
previousPose_(Transform::getIdentity())
#endif
{
#ifdef RTABMAP_VINS
#ifdef RTABMAP_VINS_FUSION
// intialize
std::string configFilename;
Parameters::parse(parameters, Parameters::kOdomVINSConfigPath(), configFilename);
Parameters::parse(parameters, Parameters::kOdomVINSFusionConfigPath(), configFilename);
if(configFilename.empty())
{
UERROR("VINS config file is empty (%s=%s)!",
Parameters::kOdomVINSConfigPath().c_str(),
Parameters::kOdomVINSConfigPath().c_str());
UERROR("VINS config file is empty (%s)!",
Parameters::kOdomVINSFusionConfigPath().c_str());
}
else
{
UINFO("Using config file %s", configFilename.c_str());
readParameters(uReplaceChar(configFilename, '~', UDirectory::homeDir()));
}
#endif
}
OdometryVINS::~OdometryVINS()
OdometryVINSFusion::~OdometryVINSFusion()
{
#ifdef RTABMAP_VINS
#ifdef RTABMAP_VINS_FUSION
delete vinsEstimator_;
#endif
}
void OdometryVINS::reset(const Transform & initialPose)
void OdometryVINSFusion::reset(const Transform & initialPose)
{
Odometry::reset(initialPose);
#ifdef RTABMAP_VINS
#ifdef RTABMAP_VINS_FUSION
if(!initGravity_)
{
delete vinsEstimator_;
@@ -333,18 +301,19 @@ void OdometryVINS::reset(const Transform & initialPose)
}
// return not null transform if odometry is correctly computed
Transform OdometryVINS::computeTransform(
Transform OdometryVINSFusion::computeTransform(
SensorData & data,
const Transform & guess,
OdometryInfo * info)
{
Transform t;
#ifdef RTABMAP_VINS
#ifdef RTABMAP_VINS_FUSION
UTimer timer;
bool hasImage = !data.imageRaw().empty() && !data.rightRaw().empty() && data.stereoCameraModels().size() == 1 && data.stereoCameraModels()[0].isValidForProjection();
if(USE_IMU!=0 && !data.imu().empty())
{
double t = data.stamp();
double dx = data.imu().linearAcceleration().val[0];
double dy = data.imu().linearAcceleration().val[1];
double dz = data.imu().linearAcceleration().val[2];
@@ -358,16 +327,19 @@ Transform OdometryVINS::computeTransform(
if(vinsEstimator_ != 0)
{
vinsEstimator_->inputIMU(t, acc, gyr);
vinsEstimator_->inputIMU(data.stamp(), acc, gyr);
}
else
{
lastImu_ = data.imu();
UWARN("Waiting an image for initialization...");
lastImuStamp_ = data.stamp();
if(!hasImage) {
UWARN("Waiting an image for initialization...");
}
}
}
if(!data.imageRaw().empty() && !data.rightRaw().empty() && data.stereoCameraModels().size() == 1 && data.stereoCameraModels()[0].isValidForProjection())
if(hasImage)
{
if(USE_IMU==1 && lastImu_.localTransform().isNull())
{
@@ -377,10 +349,23 @@ Transform OdometryVINS::computeTransform(
if(vinsEstimator_ == 0)
{
// intialize
vinsEstimator_ = new VinsEstimator(
UINFO("Initializing with image %f", data.stamp());
vinsEstimator_ = new VinsFusionEstimator(
lastImu_.localTransform().isNull()?Transform::getIdentity():lastImu_.localTransform(),
data.stereoCameraModels()[0],
this->imagesAlreadyRectified());
if(USE_IMU) {
double dx = lastImu_.linearAcceleration().val[0];
double dy = lastImu_.linearAcceleration().val[1];
double dz = lastImu_.linearAcceleration().val[2];
double rx = lastImu_.angularVelocity().val[0];
double ry = lastImu_.angularVelocity().val[1];
double rz = lastImu_.angularVelocity().val[2];
Vector3d acc(dx, dy, dz);
Vector3d gyr(rx, ry, rz);
vinsEstimator_->inputIMU(lastImuStamp_, acc, gyr);
}
}
UDEBUG("Image update stamp=%f", data.stamp());
@@ -455,36 +440,45 @@ Transform OdometryVINS::computeTransform(
info->reg.covariance = cv::Mat::eye(6,6, CV_64FC1);
info->reg.covariance *= this->framesProcessed() == 0?9999:0.0001;
// feature map
Transform fixT = this->getPose()*previousPoseInv;
// feature map: based on code from pubPointCloud() of vins's visualization.cpp
for (auto &it_per_id : vinsEstimator_->f_manager.feature)
{
int used_num;
used_num = it_per_id.feature_per_frame.size();
if (!(used_num >= 2 && it_per_id.start_frame < WINDOW_SIZE - 2))
continue;
if (it_per_id.start_frame > WINDOW_SIZE * 3.0 / 4.0 || it_per_id.solve_flag != 1)
if(it_per_id.feature_per_frame.size() < 2) {
// feature just added but not tracked, or old feature not tracked anymore
continue;
}
int imu_i = it_per_id.start_frame;
Vector3d pts_i = it_per_id.feature_per_frame[it_per_id.feature_per_frame.size()-1].point * it_per_id.estimated_depth;
Vector3d pts_i = it_per_id.feature_per_frame[0].point * it_per_id.estimated_depth;
Vector3d w_pts_i = vinsEstimator_->Rs[imu_i] * (vinsEstimator_->ric[0] * pts_i + vinsEstimator_->tic[0]) + vinsEstimator_->Ps[imu_i];
cv::Point3f p;
p.x = w_pts_i(0);
p.y = w_pts_i(1);
p.z = w_pts_i(2);
p = util3d::transformPoint(p, fixT);
info->localMap.insert(std::make_pair(it_per_id.feature_id, p));
if(this->imagesAlreadyRectified())
{
cv::Point2f pt;
data.stereoCameraModels()[0].left().reproject(pts_i(0), pts_i(1), pts_i(2), pt.x, pt.y);
info->reg.inliersIDs.push_back(info->newCorners.size());
info->newCorners.push_back(pt);
int featureIndex = info->localMap.size();
info->localMap.insert(std::make_pair(featureIndex, p));
FeaturePerFrame & refFrame = it_per_id.feature_per_frame[0]; // First frame it was seen
FeaturePerFrame & newFrame = it_per_id.feature_per_frame[it_per_id.feature_per_frame.size()-1]; // Last frame it was seen (not necessary in last frame)
cv::Point2f refUV(refFrame.uv[0], refFrame.uv[1]);
cv::Point2f newUV(newFrame.uv[0], newFrame.uv[1]);
info->refCorners.push_back(refUV);
info->newCorners.push_back(newUV);
info->reg.matchesIDs.push_back(featureIndex);
if(it_per_id.solve_flag > 0) {
// Feature correctly tracked
info->words.insert(std::make_pair(featureIndex, cv::KeyPoint(newUV, 3.0f)));
info->cornerInliers.push_back(featureIndex);
info->reg.inliersIDs.push_back(featureIndex);
}
++featureIndex;
}
info->features = info->newCorners.size();
info->features = info->localMap.size();
info->reg.inliers = info->reg.inliersIDs.size();
info->localMapSize = info->localMap.size();
}
UINFO("Odom update time = %fs p=%s", timer.elapsed(), p.prettyPrint().c_str());
@@ -492,7 +486,7 @@ Transform OdometryVINS::computeTransform(
}
else
{
UWARN("VINS not yet initialized... waiting to get enough IMU messages");
UWARN("VINS-Fusion not yet initialized... needing more data.");
}
}
else if(!data.imageRaw().empty() && !data.depthRaw().empty())