Fixed a realsense2 camera crash on start. Refactored VINS-Fusion to always use rtabmap's calibration model no matter what (improved usability). Preferences/IMU: fixed publish inter imu checkbox disappearing if no imu filter is selected.

This commit is contained in:
matlabbe
2025-11-01 13:07:50 -07:00
parent b04f607828
commit a20e161a5b
5 changed files with 170 additions and 92 deletions

View File

@@ -960,7 +960,6 @@ void SensorCaptureThread::postUpdate(SensorData * dataPtr, SensorCaptureInfo * i
{ {
UASSERT(!data.imu().localTransform().isNull()); UASSERT(!data.imu().localTransform().isNull());
imu.convertToBaseFrame(); imu.convertToBaseFrame();
} }
_imuFilter->update( _imuFilter->update(
imu.angularVelocity()[0], imu.angularVelocity()[0],

View File

@@ -712,14 +712,14 @@ bool CameraRealSense2::init(const std::string & calibrationFolder, const std::st
for (auto& profile : profiles) for (auto& profile : profiles)
{ {
auto video_profile = profile.as<rs2::video_stream_profile>(); auto video_profile = profile.as<rs2::video_stream_profile>();
UINFO("%s %d %d %d %d %s type=%d", rs2_format_to_string( UINFO("%s %d %d %d %d %s type=%d",
video_profile.format()), rs2_format_to_string(profile.format()),
video_profile.width(), video_profile.get()?video_profile.width():-1,
video_profile.height(), video_profile.get()?video_profile.height():-1,
video_profile.fps(), profile.fps(),
video_profile.stream_index(), profile.stream_index(),
video_profile.stream_name().c_str(), profile.stream_name().c_str(),
video_profile.stream_type()); profile.stream_type());
} }
} }
int pi = 0; int pi = 0;
@@ -728,7 +728,8 @@ bool CameraRealSense2::init(const std::string & calibrationFolder, const std::st
auto video_profile = profile.as<rs2::video_stream_profile>(); auto video_profile = profile.as<rs2::video_stream_profile>();
if(!stereo) if(!stereo)
{ {
if( (video_profile.width() == cameraWidth_ && if( (video_profile.get() &&
video_profile.width() == cameraWidth_ &&
video_profile.height() == cameraHeight_ && video_profile.height() == cameraHeight_ &&
video_profile.fps() == cameraFps_) || video_profile.fps() == cameraFps_) ||
(strcmp(sensors[i].get_info(RS2_CAMERA_INFO_NAME), "L500 Depth Sensor")==0 && (strcmp(sensors[i].get_info(RS2_CAMERA_INFO_NAME), "L500 Depth Sensor")==0 &&
@@ -778,7 +779,7 @@ bool CameraRealSense2::init(const std::string & calibrationFolder, const std::st
} }
} }
} }
else if(video_profile.format() == RS2_FORMAT_MOTION_XYZ32F || video_profile.format() == RS2_FORMAT_6DOF) else if(profile.format() == RS2_FORMAT_MOTION_XYZ32F || profile.format() == RS2_FORMAT_6DOF)
{ {
//D435i: //D435i:
//MOTION_XYZ32F 0 0 200 (gyro) //MOTION_XYZ32F 0 0 200 (gyro)
@@ -817,6 +818,7 @@ bool CameraRealSense2::init(const std::string & calibrationFolder, const std::st
{ {
//T265: //T265:
if(!dualMode_ && if(!dualMode_ &&
video_profile.get() &&
video_profile.format() == RS2_FORMAT_Y8 && video_profile.format() == RS2_FORMAT_Y8 &&
video_profile.width() == 848 && video_profile.width() == 848 &&
video_profile.height() == 800 && video_profile.height() == 800 &&
@@ -865,7 +867,7 @@ bool CameraRealSense2::init(const std::string & calibrationFolder, const std::st
} }
added = true; added = true;
} }
else if(video_profile.format() == RS2_FORMAT_MOTION_XYZ32F || video_profile.format() == RS2_FORMAT_6DOF) else if(profile.format() == RS2_FORMAT_MOTION_XYZ32F || profile.format() == RS2_FORMAT_6DOF)
{ {
//MOTION_XYZ32F 0 0 200 //MOTION_XYZ32F 0 0 200
//MOTION_XYZ32F 0 0 62 //MOTION_XYZ32F 0 0 62
@@ -884,14 +886,14 @@ bool CameraRealSense2::init(const std::string & calibrationFolder, const std::st
for (auto& profile : profiles) for (auto& profile : profiles)
{ {
auto video_profile = profile.as<rs2::video_stream_profile>(); auto video_profile = profile.as<rs2::video_stream_profile>();
UERROR("%s %d %d %d %d %s type=%d", rs2_format_to_string( UERROR("%s %d %d %d %d %s type=%d",
video_profile.format()), rs2_format_to_string(profile.format()),
video_profile.width(), video_profile.get()?video_profile.width():-1,
video_profile.height(), video_profile.get()?video_profile.height():-1,
video_profile.fps(), profile.fps(),
video_profile.stream_index(), profile.stream_index(),
video_profile.stream_name().c_str(), profile.stream_name().c_str(),
video_profile.stream_type()); profile.stream_type());
} }
return false; return false;
} }
@@ -1075,13 +1077,13 @@ bool CameraRealSense2::init(const std::string & calibrationFolder, const std::st
{ {
auto video_profile = profilesPerSensor[i][j].as<rs2::video_stream_profile>(); auto video_profile = profilesPerSensor[i][j].as<rs2::video_stream_profile>();
UINFO("Opening: %s %d %d %d %d %s type=%d", rs2_format_to_string( UINFO("Opening: %s %d %d %d %d %s type=%d", rs2_format_to_string(
video_profile.format()), profilesPerSensor[i][j].format()),
video_profile.width(), video_profile.get()?video_profile.width():-1,
video_profile.height(), video_profile.get()?video_profile.height():-1,
video_profile.fps(), profilesPerSensor[i][j].fps(),
video_profile.stream_index(), profilesPerSensor[i][j].stream_index(),
video_profile.stream_name().c_str(), profilesPerSensor[i][j].stream_name().c_str(),
video_profile.stream_type()); profilesPerSensor[i][j].stream_type());
} }
if(globalTimeSync_ && sensors[i].supports(rs2_option::RS2_OPTION_GLOBAL_TIME_ENABLED)) if(globalTimeSync_ && sensors[i].supports(rs2_option::RS2_OPTION_GLOBAL_TIME_ENABLED))
{ {
@@ -1525,6 +1527,13 @@ SensorData CameraRealSense2::captureImage(SensorCaptureInfo * info)
else else
{ {
UERROR("Missing frames (received %d, needed=%d)", (int)frameset.size(), desiredFramesetSize); UERROR("Missing frames (received %d, needed=%d)", (int)frameset.size(), desiredFramesetSize);
if(frameset.size()>0)
{
for (auto it = frameset.begin(); it != frameset.end(); ++it)
{
UERROR("Received frame only from %s", (*it).get_profile().stream_name().c_str());
}
}
} }
} }
catch(const std::exception& ex) catch(const std::exception& ex)

View File

@@ -39,7 +39,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
#include <estimator/estimator.h> #include <estimator/estimator.h>
#include <estimator/parameters.h> #include <estimator/parameters.h>
#include <camodocal/camera_models/PinholeCamera.h> #include <camodocal/camera_models/PinholeCamera.h>
#include <camodocal/camera_models/EquidistantCamera.h> #include <camodocal/camera_models/PinholeFullCamera.h>
#include <utility/visualization.h> #include <utility/visualization.h>
#endif #endif
@@ -49,10 +49,12 @@ namespace rtabmap {
class VinsFusionEstimator: public Estimator class VinsFusionEstimator: public Estimator
{ {
public: public:
VinsFusionEstimator( VinsFusionEstimator() : Estimator()
const Transform & imuLocalTransform, {}
bool init(const Transform & imuLocalTransform,
const StereoCameraModel & model, const StereoCameraModel & model,
bool rectified) : Estimator() bool rectified)
{ {
MULTIPLE_THREAD = 0; MULTIPLE_THREAD = 0;
setParameter(); setParameter();
@@ -63,71 +65,133 @@ public:
//overwrite camera calibration only if received model is radtan, otherwise use config //overwrite camera calibration only if received model is radtan, otherwise use config
UASSERT(NUM_OF_CAM >= 1 && NUM_OF_CAM <=2); UASSERT(NUM_OF_CAM >= 1 && NUM_OF_CAM <=2);
if( (NUM_OF_CAM == 2 && model.left().D_raw().cols == 4 && model.right().D_raw().cols == 4) || if( (NUM_OF_CAM == 2 && (rectified || (model.left().D_raw().cols >= 4 && model.right().D_raw().cols >= 4))) ||
(NUM_OF_CAM == 1 && model.left().D_raw().cols == 4)) (NUM_OF_CAM == 1 && (rectified || model.left().D_raw().cols >= 4)))
{ {
UWARN("Overwriting VINS camera calibration config with received pinhole model... rectified=%d", rectified?1:0); UINFO("Setting up VINS camera calibration config with received pinhole model... rectified=%d distortion coefficients=%d",
rectified?1:0, model.left().D_raw().cols);
featureTracker.m_camera.clear(); featureTracker.m_camera.clear();
camodocal::PinholeCameraPtr camera( new camodocal::PinholeCamera ); double fx = 0.0;
camodocal::PinholeCamera::Parameters params( if(!rectified && model.left().D_raw().cols >= 8)
model.name(), {
model.left().imageWidth(), model.left().imageHeight(), if(model.left().D_raw().cols > 8)
rectified?0:model.left().D_raw().at<double>(0,0), {
rectified?0:model.left().D_raw().at<double>(0,1), UWARN("Received %d distortion coefficients, but only the first 8 are supported, ignoring the last coefficents.",
rectified?0:model.left().D_raw().at<double>(0,2), model.left().D_raw().cols);
rectified?0:model.left().D_raw().at<double>(0,3), }
rectified?model.left().fx():model.left().K_raw().at<double>(0,0), camodocal::PinholeFullCameraPtr camera( new camodocal::PinholeFullCamera );
rectified?model.left().fy():model.left().K_raw().at<double>(1,1), camodocal::PinholeFullCamera::Parameters params(
rectified?model.left().cx():model.left().K_raw().at<double>(0,2), model.name(),
rectified?model.left().cy():model.left().K_raw().at<double>(1,2)); model.left().imageWidth(), model.left().imageHeight(),
camera->setParameters(params); model.left().D_raw().at<double>(0,0), // k1
featureTracker.m_camera.push_back(camera); model.left().D_raw().at<double>(0,1), // k1
model.left().D_raw().at<double>(0,4), // k3
model.left().D_raw().at<double>(0,5), // k4
model.left().D_raw().at<double>(0,6), // k5
model.left().D_raw().at<double>(0,7), // k6
model.left().D_raw().at<double>(0,2), // p1
model.left().D_raw().at<double>(0,3), // p1
model.left().K_raw().at<double>(0,0), // fx
model.left().K_raw().at<double>(1,1), // fy
model.left().K_raw().at<double>(0,2), // cx
model.left().K_raw().at<double>(1,2)); // cy
camera->setParameters(params);
featureTracker.m_camera.push_back(camera);
fx = params.fx();
if(NUM_OF_CAM == 2)
{
UASSERT(model.left().D_raw().cols == model.right().D_raw().cols);
camodocal::PinholeFullCameraPtr camera2( new camodocal::PinholeFullCamera );
camodocal::PinholeFullCamera::Parameters params2(
model.name(),
model.right().imageWidth(), model.right().imageHeight(),
model.right().D_raw().at<double>(0,0), // k1
model.right().D_raw().at<double>(0,1), // k2
model.right().D_raw().at<double>(0,4), // k3
model.right().D_raw().at<double>(0,5), // k4
model.right().D_raw().at<double>(0,6), // k5
model.right().D_raw().at<double>(0,7), // k6
model.right().D_raw().at<double>(0,2), // p1
model.right().D_raw().at<double>(0,3), // p2
model.right().K_raw().at<double>(0,0), // fx
model.right().K_raw().at<double>(1,1), // fy
model.right().K_raw().at<double>(0,2), // cx
model.right().K_raw().at<double>(1,2)); // cy
camera2->setParameters(params2);
featureTracker.m_camera.push_back(camera2);
}
}
else
{
if(!rectified)
{
if(model.left().D_raw().cols == 6) {
UERROR("Fisheye camera model support not implemented! Provide rectified images instead (see %s).",
Parameters::kRtabmapImagesAlreadyRectified().c_str());
return false;
}
if(model.left().D_raw().cols > 4)
{
UWARN("Received %d distortion coefficients, but only 4 or 8 are supported, ignoring the last coefficents.",
model.left().D_raw().cols);
}
}
camodocal::PinholeCameraPtr camera( new camodocal::PinholeCamera );
camodocal::PinholeCamera::Parameters params(
model.name(),
model.left().imageWidth(), model.left().imageHeight(),
rectified?0:model.left().D_raw().at<double>(0,0), // k1
rectified?0:model.left().D_raw().at<double>(0,1), // k2
rectified?0:model.left().D_raw().at<double>(0,2), // p1
rectified?0:model.left().D_raw().at<double>(0,3), // p2
rectified?model.left().fx():model.left().K_raw().at<double>(0,0),
rectified?model.left().fy():model.left().K_raw().at<double>(1,1),
rectified?model.left().cx():model.left().K_raw().at<double>(0,2),
rectified?model.left().cy():model.left().K_raw().at<double>(1,2));
camera->setParameters(params);
featureTracker.m_camera.push_back(camera);
fx = params.fx();
if(NUM_OF_CAM == 2)
{
UASSERT(model.left().D_raw().cols == model.right().D_raw().cols);
camodocal::PinholeCameraPtr camera2( new camodocal::PinholeCamera );
camodocal::PinholeCamera::Parameters params2(
model.name(),
model.right().imageWidth(), model.right().imageHeight(),
rectified?0:model.right().D_raw().at<double>(0,0), // k1
rectified?0:model.right().D_raw().at<double>(0,1), // k2
rectified?0:model.right().D_raw().at<double>(0,2), // p1
rectified?0:model.right().D_raw().at<double>(0,3), // p2
rectified?model.right().fx():model.right().K_raw().at<double>(0,0),
rectified?model.right().fy():model.right().K_raw().at<double>(1,1),
rectified?model.right().cx():model.right().K_raw().at<double>(0,2),
rectified?model.right().cy():model.right().K_raw().at<double>(1,2));
camera2->setParameters(params2);
featureTracker.m_camera.push_back(camera2);
}
}
double originalParalax = MIN_PARALLAX * FOCAL_LENGTH; double originalParalax = MIN_PARALLAX * FOCAL_LENGTH;
// If you have compiler error about FOCAL_LENGTH being const, make sure to use the following patch: // If you have compiler error about FOCAL_LENGTH being const, make sure to use the following patch for ROS1:
// https://gist.github.com/matlabbe/795ab37067367dca58bbadd8201d986c#file-vins-fusion_pull136-patch // https://gist.github.com/matlabbe/795ab37067367dca58bbadd8201d986c#file-vins-fusion_pull136-patch
FOCAL_LENGTH = params.fx(); // Use this patch for ROS2: https://gist.github.com/matlabbe/ebbb343cd744da9d6d6d6ded2e1557fd
FOCAL_LENGTH = fx;
MIN_PARALLAX = originalParalax / FOCAL_LENGTH; MIN_PARALLAX = originalParalax / FOCAL_LENGTH;
ProjectionTwoFrameOneCamFactor::sqrt_info = FOCAL_LENGTH / 1.5 * Matrix2d::Identity(); ProjectionTwoFrameOneCamFactor::sqrt_info = FOCAL_LENGTH / 1.5 * Matrix2d::Identity();
ProjectionTwoFrameTwoCamFactor::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(); ProjectionOneFrameTwoCamFactor::sqrt_info = FOCAL_LENGTH / 1.5 * Matrix2d::Identity();
if(NUM_OF_CAM == 2)
{
camodocal::PinholeCameraPtr camera( new camodocal::PinholeCamera );
camodocal::PinholeCamera::Parameters params(
model.name(),
model.right().imageWidth(), model.right().imageHeight(),
rectified?0:model.right().D_raw().at<double>(0,0),
rectified?0:model.right().D_raw().at<double>(0,1),
rectified?0:model.right().D_raw().at<double>(0,2),
rectified?0:model.right().D_raw().at<double>(0,3),
rectified?model.right().fx():model.right().K_raw().at<double>(0,0),
rectified?model.right().fy():model.right().K_raw().at<double>(1,1),
rectified?model.right().cx():model.right().K_raw().at<double>(0,2),
rectified?model.right().cy():model.right().K_raw().at<double>(1,2));
camera->setParameters(params);
featureTracker.m_camera.push_back(camera);
}
} }
else if(rectified) else
{ {
UWARN("Images are rectified but received calibration cannot be " UERROR("Received stereo camera model is not compatible with VINS-Fusion.");
"used, make sure calibration in config file doesn't have " if(!rectified && model.left().D_raw().cols != 4) {
"distortion or send raw images to VINS odometry."); UERROR("When raw images are provided (%s=false), we expect 4 distortion coefficients (k1,k2,p1,p2), received %d",
if(!featureTracker.m_camera.empty()) Parameters::kRtabmapImagesAlreadyRectified().c_str(),
{ model.left().D_raw().cols);
if(featureTracker.m_camera.front()->imageWidth() != model.left().imageWidth() ||
featureTracker.m_camera.front()->imageHeight() != model.left().imageHeight())
{
UERROR("Received images don't have same size (%dx%d) than in the config file (%dx%d)!",
model.left().imageWidth(),
model.left().imageHeight(),
featureTracker.m_camera.front()->imageWidth(),
featureTracker.m_camera.front()->imageHeight());
}
} }
return false;
} }
Transform imuCam0 = imuLocalTransform.inverse() * model.localTransform(); Transform imuCam0 = imuLocalTransform.inverse() * model.localTransform();
@@ -165,6 +229,7 @@ public:
cout << " new intrinsic cam " << i << endl << featureTracker.m_camera[i]->parametersToString() << endl; cout << " new intrinsic cam " << i << endl << featureTracker.m_camera[i]->parametersToString() << endl;
} }
f_manager.setRic(ric); f_manager.setRic(ric);
return true;
} }
// Copy of original inputImage() so that overridden processMeasurements() is used and threading is disabled. // Copy of original inputImage() so that overridden processMeasurements() is used and threading is disabled.
@@ -350,10 +415,16 @@ Transform OdometryVINSFusion::computeTransform(
{ {
// intialize // intialize
UINFO("Initializing with image %f", data.stamp()); UINFO("Initializing with image %f", data.stamp());
vinsEstimator_ = new VinsFusionEstimator( vinsEstimator_ = new VinsFusionEstimator();
lastImu_.localTransform().isNull()?Transform::getIdentity():lastImu_.localTransform(), if(!vinsEstimator_->init(
data.stereoCameraModels()[0], lastImu_.localTransform().isNull()?Transform::getIdentity():lastImu_.localTransform(),
this->imagesAlreadyRectified()); data.stereoCameraModels()[0],
this->imagesAlreadyRectified()))
{
delete vinsEstimator_;
vinsEstimator_ = 0;
return Transform();
}
if(USE_IMU) { if(USE_IMU) {
double dx = lastImu_.linearAcceleration().val[0]; double dx = lastImu_.linearAcceleration().val[0];

View File

@@ -5868,7 +5868,6 @@ void PreferencesDialog::updateSourceGrpVisibility()
(_ui->comboBox_sourceType->currentIndex() == 1 && _ui->comboBox_cameraStereo->currentIndex() == kSrcStereoMyntEye - kSrcStereo) || // MYNT EYE S (_ui->comboBox_sourceType->currentIndex() == 1 && _ui->comboBox_cameraStereo->currentIndex() == kSrcStereoMyntEye - kSrcStereo) || // MYNT EYE S
(_ui->comboBox_sourceType->currentIndex() == 1 && _ui->comboBox_cameraStereo->currentIndex() == kSrcStereoZedOC - kSrcStereo) || (_ui->comboBox_sourceType->currentIndex() == 1 && _ui->comboBox_cameraStereo->currentIndex() == kSrcStereoZedOC - kSrcStereo) ||
(_ui->comboBox_sourceType->currentIndex() == 1 && _ui->comboBox_cameraStereo->currentIndex() == kSrcStereoDepthAI - kSrcStereo)); (_ui->comboBox_sourceType->currentIndex() == 1 && _ui->comboBox_cameraStereo->currentIndex() == kSrcStereoDepthAI - kSrcStereo));
_ui->frame_imu_filtering->setVisible(getIMUFilteringStrategy() > 0); // Not None
_ui->stackedWidget_imuFilter->setVisible(_ui->comboBox_imuFilter_strategy->currentIndex() > 0); _ui->stackedWidget_imuFilter->setVisible(_ui->comboBox_imuFilter_strategy->currentIndex() > 0);
_ui->groupBox_madgwickfilter->setVisible(_ui->comboBox_imuFilter_strategy->currentIndex() == 1); _ui->groupBox_madgwickfilter->setVisible(_ui->comboBox_imuFilter_strategy->currentIndex() == 1);
_ui->groupBox_complementaryfilter->setVisible(_ui->comboBox_imuFilter_strategy->currentIndex() == 2); _ui->groupBox_complementaryfilter->setVisible(_ui->comboBox_imuFilter_strategy->currentIndex() == 2);
@@ -6706,7 +6705,7 @@ Camera * PreferencesDialog::createCamera(
((CameraOrbbecSDK*)camera)->enableColorRectification(_ui->checkBox_orbbec_sdk_color_rectification->isChecked()); ((CameraOrbbecSDK*)camera)->enableColorRectification(_ui->checkBox_orbbec_sdk_color_rectification->isChecked());
((CameraOrbbecSDK*)camera)->enableImu(_ui->checkBox_orbbec_sdk_imu->isChecked()); ((CameraOrbbecSDK*)camera)->enableImu(_ui->checkBox_orbbec_sdk_imu->isChecked());
((CameraOrbbecSDK*)camera)->enableDepthMM(_ui->checkBox_orbbec_sdk_depth_mm->isChecked()); ((CameraOrbbecSDK*)camera)->enableDepthMM(_ui->checkBox_orbbec_sdk_depth_mm->isChecked());
camera->setInterIMUPublishing( camera->setInterIMUPublishing(
_ui->checkbox_publishInterIMU->isChecked(), _ui->checkbox_publishInterIMU->isChecked(),
_ui->checkbox_publishInterIMU->isChecked() && getIMUFilteringStrategy()>0? _ui->checkbox_publishInterIMU->isChecked() && getIMUFilteringStrategy()>0?

View File

@@ -63,7 +63,7 @@
<property name="geometry"> <property name="geometry">
<rect> <rect>
<x>0</x> <x>0</x>
<y>0</y> <y>-2651</y>
<width>713</width> <width>713</width>
<height>4779</height> <height>4779</height>
</rect> </rect>
@@ -95,7 +95,7 @@
<enum>QFrame::Raised</enum> <enum>QFrame::Raised</enum>
</property> </property>
<property name="currentIndex"> <property name="currentIndex">
<number>12</number> <number>5</number>
</property> </property>
<widget class="QWidget" name="page_22"> <widget class="QWidget" name="page_22">
<layout class="QVBoxLayout" name="verticalLayout_29" stretch="0,0"> <layout class="QVBoxLayout" name="verticalLayout_29" stretch="0,0">
@@ -8283,7 +8283,7 @@ when using the file type, logs are saved in LogRtabmap.txt (located in the worki
</widget> </widget>
</item> </item>
<item row="0" column="1"> <item row="0" column="1">
<widget class="QLabel" name="label_621"> <widget class="QLabel" name="label_imuFilter_baseFrameConversion">
<property name="text"> <property name="text">
<string>Convert IMU in base frame before filtering. This can help to initialize correctly the yaw.</string> <string>Convert IMU in base frame before filtering. This can help to initialize correctly the yaw.</string>
</property> </property>