search

jeremysalwen / ORB_SLAM_COMMUNITY

A community maintained fork of the inactive ORB_SLAM3 project
GNU General Public License v3.0
14 stars 2 forks source link

stereo camera Tracking not initialized #4

Open nesquik011 opened 3 weeks ago

nesquik011 commented 3 weeks ago

it keep waiting for images , i hate this msg , i am stuck there for days , here some of the prints : Actual camera settings: Width: 3840 Height: 1080 FPS: 60 Input sensor was set to: Stereo

Loading ORB Vocabulary. This could take a while... Vocabulary loaded!

Initialization of Atlas from scratch Creation of new map with id: 0 Creation of new map with last KF id: 0 Seq. Name:

Camera Parameters:

Depth Threshold (Close/Far Points): 0.602271

ORB Extractor Parameters:

Start processing live feed ... Starting the Viewer Current timestamp: 0.476911 seconds Tracking not initialized. Current timestamp: 19.116371 seconds Tracking not initialized. Current timestamp: 19.333230 seconds Tracking not initialized. Current timestamp: 19.442575 seconds Tracking not initialized. Current timestamp: 19.498325 seconds Tracking not initialized.

the CODE starts Here : `#include

include

include

include

include

include

include <opencv2/core.hpp>

include <opencv2/highgui.hpp>

include <opencv2/calib3d.hpp>

include <opencv2/videoio.hpp>

include "System.h" // Adjust path as per your project setup

using namespace std;

void LoadCameraParameters(const string& pathToSettings, cv::Mat& K_left, cv::Mat& K_right, cv::Mat& D_left, cv::Mat& D_right, cv::Mat& R, cv::Mat& T, cv::Size& imageSize, double& fps, double& bf);

int main(int argc, char** argv) { if (argc != 4) { cerr << endl << "Usage: ./stereo_live path_to_vocabulary path_to_settings camera_index" << endl; return 1; }

int cameraIndex = atoi(argv[3]);

// Open the camera using DirectShow API (specific to Windows)
cv::VideoCapture cap(cameraIndex);
if (!cap.isOpened())
{
    cerr << "Error: Failed to open the camera." << endl;
    return -1;
}

// Set the desired capture properties
cap.set(cv::CAP_PROP_FRAME_WIDTH, 3840);
cap.set(cv::CAP_PROP_FRAME_HEIGHT, 1080);
cap.set(cv::CAP_PROP_FPS, 60.0);

// Capture actual camera settings
double actualWidth = cap.get(cv::CAP_PROP_FRAME_WIDTH);
double actualHeight = cap.get(cv::CAP_PROP_FRAME_HEIGHT);
double actualFPS = cap.get(cv::CAP_PROP_FPS);

// Display the actual camera settings
cout << "Actual camera settings:" << endl;
cout << "Width: " << actualWidth << endl;
cout << "Height: " << actualHeight << endl;
cout << "FPS: " << actualFPS << endl;

// Verify if the camera settings match the expected values
if (abs(actualWidth - 3840) > 1 || abs(actualHeight - 1080) > 1 || abs(actualFPS - 60.0) > 1)
{
    cerr << "Error: Camera settings do not match expected values." << endl;
    return -1;
}

// Initialize matrices for stereo calibration
cv::Mat K_left = cv::Mat::eye(3, 3, CV_64F);
cv::Mat K_right = cv::Mat::eye(3, 3, CV_64F);
cv::Mat D_left = cv::Mat::zeros(1, 4, CV_64F);
cv::Mat D_right = cv::Mat::zeros(1, 4, CV_64F);
cv::Mat R = cv::Mat::eye(3, 3, CV_64F);
cv::Mat T = cv::Mat::zeros(3, 1, CV_64F);
cv::Size imageSize(1280, 720);  // Initial size, will be updated later
double fps, bf;

// Load camera parameters from the settings file
string pathToSettings = argv[2];
LoadCameraParameters(pathToSettings, K_left, K_right, D_left, D_right, R, T, imageSize, fps, bf);

// Scale camera matrices for 1920x1080 images
//double scale_x = 1920.0 / imageSize.width;
//double scale_y = 1080.0 / imageSize.height;
//K_left.at<double>(0, 0) *= scale_x;
//K_left.at<double>(1, 1) *= scale_y;
//K_left.at<double>(0, 2) *= scale_x;
//K_left.at<double>(1, 2) *= scale_y;
K_right = K_left.clone();

// Perform stereo rectification
cv::Mat R1, R2, P1, P2, Q;
cv::Mat map1_left, map2_left, map1_right, map2_right;
cv::stereoRectify(K_left, D_left, K_right, D_right, imageSize, R, T, R1, R2, P1, P2, Q);

// Initialize undistort rectify maps
cv::initUndistortRectifyMap(K_left, D_left, R1, P1, imageSize, CV_32FC1, map1_left, map2_left);
cv::initUndistortRectifyMap(K_right, D_right, R2, P2, imageSize, CV_32FC1, map1_right, map2_right);

// Initialize the ORB_SLAM3 system
ORB_SLAM3::System SLAM(argv[1], argv[2], ORB_SLAM3::System::STEREO, true);

cout << endl << "-------" << endl;
cout << "Start processing live feed ..." << endl;

vector<float> vTimesTrack;
cv::Mat frame;
double timestamp = 0.0;
chrono::steady_clock::time_point start_time = chrono::steady_clock::now();

// Main loop to process the video feed
while (true)
{
    cap >> frame;
    if (frame.empty())
    {
        cerr << "Error: Blank frame captured." << endl;
        break;
    }

    // Ensure the captured frame matches the expected size
    if (frame.size() != cv::Size(3840, 1080))
    {
        cerr << "Error: Captured frame size mismatch." << endl;
        break;
    }

    // Resize the frame
    cv::Mat resizedFrame;
    cv::resize(frame, resizedFrame, cv::Size(2560, 720));

    // Split the resized frame into left and right images
    cv::Mat imLeft = resizedFrame(cv::Rect(0, 0, 1280, 720));
    cv::Mat imRight = resizedFrame(cv::Rect(1280, 0, 1280, 720));

    // Rectify the images
    cv::Mat rectifiedLeft, rectifiedRight;
    cv::remap(imLeft, rectifiedLeft, map1_left, map2_left, cv::INTER_LINEAR);
    cv::remap(imRight, rectifiedRight, map1_right, map2_right, cv::INTER_LINEAR);

    // Calculate timestamp
    chrono::steady_clock::time_point now = chrono::steady_clock::now();
    timestamp = chrono::duration_cast<chrono::duration<double>>(now - start_time).count();

    // Print timestamp
    cout << "Current timestamp: " << fixed << setprecision(6) << timestamp << " seconds" << endl;

    // Track the stereo images with ORB-SLAM3
    auto t1 = chrono::steady_clock::now();
    SLAM.TrackStereo(rectifiedLeft, rectifiedRight, timestamp);
    auto t2 = chrono::steady_clock::now();
    double ttrack = chrono::duration_cast<chrono::duration<double>>(t2 - t1).count();

    vTimesTrack.push_back(ttrack);

    // Check ORB-SLAM3 system state
    auto state = SLAM.GetTrackingState();
    if (state == ORB_SLAM3::Tracking::SYSTEM_NOT_READY) {
        cout << "ORB-SLAM3 system not ready. Check initialization." << endl;
    }
    else if (state == ORB_SLAM3::Tracking::NO_IMAGES_YET) {
        cout << "No images have been processed yet." << endl;
    }
    else if (state == ORB_SLAM3::Tracking::NOT_INITIALIZED) {
        cout << "Tracking not initialized." << endl;
    }
    else if (state == ORB_SLAM3::Tracking::OK) {
        cout << "Tracking OK." << endl;
    }
    else if (state == ORB_SLAM3::Tracking::LOST) {
        cout << "Tracking lost." << endl;
    }

    // Display the rectified images
    cv::imshow("Rectified Left Image", rectifiedLeft);
    cv::imshow("Rectified Right Image", rectifiedRight);

    if (cv::waitKey(1) == 27)
        break;
}

// Shutdown SLAM system
SLAM.Shutdown();

// Release the camera
cap.release();

// Calculate and display tracking time statistics
sort(vTimesTrack.begin(), vTimesTrack.end());
float totaltime = accumulate(vTimesTrack.begin(), vTimesTrack.end(), 0.0);
cout << "-------" << endl << endl;
cout << "Total frames processed: " << vTimesTrack.size() << endl;
cout << "Median tracking time per frame: " << vTimesTrack[vTimesTrack.size() / 2] << " seconds" << endl;
cout << "Mean tracking time per frame: " << totaltime / vTimesTrack.size() << " seconds" << endl;

return 0;

}

void LoadCameraParameters(const string& pathToSettings, cv::Mat& K_left, cv::Mat& K_right, cv::Mat& D_left, cv::Mat& D_right, cv::Mat& R, cv::Mat& T, cv::Size& imageSize, double& fps, double& bf) { cv::FileStorage fs(pathToSettings, cv::FileStorage::READ); if (!fs.isOpened()) { cerr << "Failed to open settings file: " << pathToSettings << endl; return; }

// Load left camera intrinsics
K_left.at<double>(0, 0) = fs["Camera.fx"];
K_left.at<double>(1, 1) = fs["Camera.fy"];
K_left.at<double>(0, 2) = fs["Camera.cx"];
K_left.at<double>(1, 2) = fs["Camera.cy"];
K_left.at<double>(2, 2) = 1.0;

// Load right camera intrinsics (same as left, since cameras are identical)
K_right = K_left.clone();  // Same as left

// Load left camera distortion coefficients
D_left.at<double>(0) = fs["Camera.k1"];
D_left.at<double>(1) = fs["Camera.k2"];
D_left.at<double>(2) = fs["Camera.p1"];
D_left.at<double>(3) = fs["Camera.p2"];

// Load right camera distortion coefficients (same as left, since cameras are identical)
D_right = D_left.clone();  // Same as left

// Load stereo calibration parameters
vector<double> Rvec(9), Tvec(3);
fs["R"] >> Rvec;
fs["T"] >> Tvec;

// Convert Rvec and Tvec to OpenCV matrices
R = cv::Mat(3, 3, CV_64F);
T = cv::Mat(3, 1, CV_64F);

std::copy(Rvec.begin(), Rvec.end(), R.ptr<double>());
std::copy(Tvec.begin(), Tvec.end(), T.ptr<double>());

// Load image size, FPS, and baseline-focal length product
imageSize.width = fs["Camera.width"];
imageSize.height = fs["Camera.height"];
fps = fs["Camera.fps"];
bf = fs["Camera.bf"];

fs.release();

}`

i am using Windoes 10 , CUDA , and it works perfectly with sequances

some info about my camera its ELP stereo camera , and no one ever think about buying this thing ever !

nesquik011 commented 3 weeks ago

image type: 8UC3 , i really dont understand , i thought maybe the rectification is bad , so i test with orb slam2 it worked perfectly , then i feeded raw images , still the same problem , but it also works well with orb slam2