mirror of
https://github.com/introlab/rtabmap.git
synced 2026-09-03 01:50:24 +08:00
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:
@@ -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],
|
||||||
|
|||||||
@@ -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)
|
||||||
|
|||||||
@@ -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];
|
||||||
|
|||||||
@@ -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?
|
||||||
|
|||||||
@@ -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>
|
||||||
|
|||||||
Reference in New Issue
Block a user