Using other SLAM

[dubing-bit]

I tried to use deep learning feature detection method to replace ORB,based on ORB-SLAM3 the modification was done and is tested feasible. I tried to compile OpenREALM using the modified ORB-SLAM3. the compilation process was smooth, including Openrealm and OpenREALM_ROS1_Bridge. unfortunately, no results were produced at the end. I think it's the SLAM used in Openrealm that is replaceable, but from compilation experience, it's usually easier to compile successfully using your RepositoriesSLAM libraries (including ORB-SLAM3 https://github.com/laxnpander/ORB_SLAM3 and OpenVSLAM https://github.com/laxnpander/openvslam ). Is there anything I should be concerned of when using other SLAM libraries?

[dubing-bit]

OpenREALM/modules/realm_vslam/realm_vslam_base/src/orb_slam.cpp

Lines 8 to 144 in 91e1e71

	OrbSlam::OrbSlam(const VisualSlamSettings::Ptr &vslam_set, const CameraSettings::Ptr &cam_set, const ImuSettings::Ptr &imu_set)
	: m_prev_keyid(-1),
	m_resizing((*vslam_set)["resizing"].toDouble()),
	m_timestamp_reference(0),
	m_path_vocabulary((*vslam_set)["path_vocabulary"].toString())
	{
	// Read the settings files
	cv::Mat K_32f = cv::Mat::eye(3, 3, CV_32F);
	K_32f.at<float>(0, 0) = (*cam_set)["fx"].toFloat() * static_cast<float>(m_resizing);
	K_32f.at<float>(1, 1) = (*cam_set)["fy"].toFloat() * static_cast<float>(m_resizing);
	K_32f.at<float>(0, 2) = (*cam_set)["cx"].toFloat() * static_cast<float>(m_resizing);
	K_32f.at<float>(1, 2) = (*cam_set)["cy"].toFloat() * static_cast<float>(m_resizing);
	cv::Mat dist_coeffs_32f = cv::Mat::zeros(1, 5, CV_32F);
	dist_coeffs_32f.at<float>(0) = (*cam_set)["k1"].toFloat();
	dist_coeffs_32f.at<float>(1) = (*cam_set)["k2"].toFloat();
	dist_coeffs_32f.at<float>(2) = (*cam_set)["p1"].toFloat();
	dist_coeffs_32f.at<float>(3) = (*cam_set)["p2"].toFloat();
	dist_coeffs_32f.at<float>(4) = (*cam_set)["k3"].toFloat();
	ORB_SLAM::CameraParameters cam{};
	cam.K = K_32f;
	cam.distCoeffs = dist_coeffs_32f;
	cam.fps = (*cam_set)["fps"].toFloat();
	cam.width = (*cam_set)["width"].toInt();
	cam.height = (*cam_set)["height"].toInt();
	cam.isRGB = false; // BGR
	ORB_SLAM::OrbParameters orb{};
	orb.nFeatures = (*vslam_set)["nrof_features"].toInt();
	orb.nLevels = (*vslam_set)["n_pyr_levels"].toInt();
	orb.scaleFactor = (*vslam_set)["scale_factor"].toFloat();
	orb.minThFast = (*vslam_set)["min_th_FAST"].toInt();
	orb.iniThFast = (*vslam_set)["ini_th_FAST"].toInt();
	#ifdef USE_ORB_SLAM2
	m_slam = new ORB_SLAM::System(m_path_vocabulary, cam, orb, ORB_SLAM::System::MONOCULAR);
	#endif
	#ifdef USE_ORB_SLAM3
	ORB_SLAM::ImuParameters imu{};
	if (imu_set != nullptr)
	{
	LOG_F(INFO, "Detected IMU settings. Loading ORB SLAM3 with IMU support.");
	imu.accelWalk = (*imu_set)["gyro_noise_density"].toFloat();
	imu.gyroWalk = (*imu_set)["gyro_bias_random_walk_noise_density"].toFloat();
	imu.noiseAccel = (*imu_set)["acc_noise_density"].toFloat();
	imu.noiseGyro = (*imu_set)["acc_bias_random_walk_noise_density"].toFloat();
	imu.Tbc = (*imu_set)["T_cam_imu"].toMat();
	imu.freq = (*imu_set)["freq"].toFloat();
	}
	if (imu_set != nullptr)
	m_slam = new ORB_SLAM::System(m_path_vocabulary, cam, imu, orb, ORB_SLAM3::System::IMU_MONOCULAR);
	else
	m_slam = new ORB_SLAM::System(m_path_vocabulary, cam, imu, orb, ORB_SLAM3::System::MONOCULAR);
	#endif
	//namespace ph = std::placeholders;
	//std::function<void(ORB_SLAM2::KeyFrame*)> kf_update = std::bind(&OrbSlam::keyframeUpdateCb, this, ph::_1);
	//_slam->RegisterKeyTransport(kf_update);
	}
	OrbSlam::~OrbSlam()
	{
	m_slam->Shutdown();
	delete m_slam;
	}
	VisualSlamIF::State OrbSlam::track(Frame::Ptr &frame, const cv::Mat &T_c2w_initial)
	{
	if (m_timestamp_reference == 0)
	{
	m_timestamp_reference = frame->getTimestamp();
	return State::LOST;
	}
	// Set image resizing accoring to settings
	frame->setImageResizeFactor(m_resizing);
	double timestamp = static_cast<double>(frame->getTimestamp() - m_timestamp_reference)/10e3;
	LOG_IF_F(INFO, true, "Time elapsed since first frame: %4.2f [s]", timestamp);
	// ORB SLAM returns a transformation from the world to the camera frame (T_w2c). In case we provide an initial guess
	// of the current pose, we have to invert this before, because in OpenREALM the standard is defined as T_c2w.
	cv::Mat T_w2c;
	#ifdef USE_ORB_SLAM2
	T_w2c = m_slam->TrackMonocular(frame->getResizedImageRaw(), timestamp);
	#endif
	#ifdef USE_ORB_SLAM3
	T_w2c = m_slam->TrackMonocular(frame->getResizedImageRaw(), timestamp, m_imu_queue);
	m_imu_queue.clear();
	#endif
	// In case tracking was successfull and slam not lost
	if (!T_w2c.empty())
	{
	// Pose definition as 3x4 matrix, calculated as 4x4 with last row (0, 0, 0, 1)
	// ORB SLAM 2 pose is defined as T_w2c, however the more intuitive way to describe
	// it for mapping is T_c2w (camera to world) therefore invert the pose matrix
	cv::Mat T_c2w = invertPose(T_w2c);
	// Also convert to double precision
	T_c2w.convertTo(T_c2w, CV_64F);
	T_c2w.pop_back();
	frame->setVisualPose(T_c2w);
	//std::cout << "Soll:\n" << T_c2w << std::endl;
	//std::cout << "Schätz:\n" << T_c2w_initial << std::endl;
	frame->setSparseCloud(getTrackedMapPoints(), true);
	// Check if new frame is keyframe by comparing current keyid with last keyid
	auto keyid = static_cast<int32_t>(m_slam->GetLastKeyFrameId());
	// Check current state of the slam
	if (m_prev_keyid == -1)
	{
	m_prev_keyid = keyid;
	return State::INITIALIZED;
	}
	else if (m_prev_keyid != keyid)
	{
	m_prev_keyid = keyid;
	m_orb_to_frame_ids.insert({keyid, frame->getFrameId()});
	return State::KEYFRAME_INSERT;
	}
	else
	{
	return State::FRAME_INSERT;
	}
	}
	return State::LOST;
	}

modify to

OrbSlam::OrbSlam(const VisualSlamSettings::Ptr &vslam_set, const CameraSettings::Ptr &cam_set, const ImuSettings::Ptr &imu_set)
: m_prev_keyid(-1),
  m_slam_setting((*vslam_set)["slam_setting"].toDouble()),
  m_resizing((*vslam_set)["resizing"].toDouble()),
  m_timestamp_reference(0),
  m_path_vocabulary((*vslam_set)["path_vocabulary"].toString())
{
  // Read the settings files
  
  m_slam = new ORB_SLAM::System(m_path_vocabulary, m_slam_setting, ORB_SLAM::System::MONOCULAR,false);
  

  //namespace ph = std::placeholders;
  //std::function<void(ORB_SLAM2::KeyFrame*)> kf_update = std::bind(&OrbSlam::keyframeUpdateCb, this, ph::_1);
  //_slam->RegisterKeyTransport(kf_update);
}

OrbSlam::~OrbSlam()
{
  m_slam->Shutdown();
  delete m_slam;
}

VisualSlamIF::State OrbSlam::track(Frame::Ptr &frame, const cv::Mat &T_c2w_initial)
{

  if (m_timestamp_reference == 0)
  {
    //Sophus::SE3f Tse1 = m_slam->TrackMonocular(frame->getImageRaw(), m_timestamp_reference, m_imu_queue);
    m_timestamp_reference = frame->getTimestamp();
    return State::LOST;
  }

  // Set image resizing accoring to settings
  // frame->setImageResizeFactor(m_resizing);
  
  double timestamp = static_cast<double>(frame->getTimestamp() - m_timestamp_reference)/10e3;
  LOG_IF_F(INFO, true, "Time elapsed since first frame: %4.2f [s]", timestamp);

  // ORB SLAM returns a transformation from the world to the camera frame (T_w2c). In case we provide an initial guess
  // of the current pose, we have to invert this before, because in OpenREALM the standard is defined as T_c2w.

  Sophus::SE3f Tse3 = m_slam->TrackMonocular(frame->getImageRaw(),timestamp);
  
  int TrackingState = m_slam->GetTrackingState();
  LOG_F(INFO,"TrackState is %u",TrackingState);

  if (!Tse3.rotationMatrix().isApprox(Eigen::Matrix3f::Identity()) || !Tse3.translation().isApprox(Eigen::Vector3f::Zero())){

  
    cv::Mat T_w2c = SE32Mat(Tse3);
    cv::Mat T_c2w = invertPose(T_w2c);
    T_c2w.convertTo(T_c2w, CV_64F);
    T_c2w.pop_back();
    frame->setVisualPose(T_c2w);

    //std::cout << "Soll:\n" << T_c2w << std::endl;
    //std::cout << "Schätz:\n" << T_c2w_initial << std::endl;

    frame->setSparseCloud(getTrackedMapPoints(), true);


    auto keyid = static_cast<int32_t>(m_slam->GetLastKeyFrameId());
    if (m_prev_keyid == -1)
    {
      m_prev_keyid = keyid;
      return State::INITIALIZED;
    }
     else if (m_prev_keyid != keyid)
    {
      m_prev_keyid = keyid;
      m_orb_to_frame_ids.insert({keyid, frame->getFrameId()});
      return State::KEYFRAME_INSERT;
    }
    else
    {
      return State::FRAME_INSERT;
    }

  }
  return State::LOST;
}

[dubing-bit]

OpenREALM/modules/realm_vslam/realm_vslam_base/src/orb_slam.cpp

Line 221 in 91e1e71

points.push_back(pt->GetWorldPos().t());

modify to

Eigen::Vector3f wp = mappoints[i]->GetWorldPos();
     
      cv::Mat p = (cv::Mat_<float>(3, 1) << wp(0), wp(1), wp(2));
      points.push_back(p);