search

youkely / InfrasCal

InfrasCal: Automatic Infrastructure-based Intrinsic and Extrinsic Calibration of a Multi-camera System
Other
228 stars 54 forks source link

How to use the RobotCar datasets for evaluation as chapter 5.5 in the paper #18

Closed ForLoveandPeace closed 1 year ago

ForLoveandPeace commented 1 year ago

Hi, thank you for great work! I got the map from RobotCar Season dataset which is of NVM format, and I turned it to the ColMap(use the script from hloc). then I downloaded the 2014/12/16 datasets in the morning as input framesets. as the paper mentioned, I choose the framesets from No.500 to frame No.900. But I can't get a good result.

Here is the result:

$ ./build/bin/infrastr_calib --camera-count 3 --output 74747B25747AE96E/loc_dataset/robotcar/2014_12_16/2014-12-16-09-14-09 --map 74747B25747AE96E/loc_dataset/overcast/demo/all_map --database 74747B25747AE96E/loc_dataset/overcast/demo/all_map/overcast-reference.db --input 74747B25747AE96E/loc_dataset/robotcar/2014_12_16/2014-12-16-09-14-09 --vocab ./data/vocabulary/sift128.bin -v --camera-model pinhole-radtan --save

# INFO: Calibration mode InRaSU # INFO: Using radial pose absolute estimator with unknown intrinsics. # INFO: Initializing... Get images from input directory # INFO: Number of frames: 400 # INFO: Loading map... Finished. # INFO: Setting up location recognition... Finished. # INFO: Initialization finished! # INFO: Wrote 0 frame sets to74747B25747AE96E/loc_dataset/robotcar/2014_12_16/2014-12-16-09-14-09/frames.sg # INFO: Number of framesets as input:400 # INFO: Pose estimation took 265.522 s. # INFO: Average radial reprojection error over all frames: nan px # INFO: Average number of frames per set: nan # ERROR: No complete frame sets were found. # INFO: Calibration took 265.522 s. terminate called after throwing an instance of 'std::out_of_range' what(): vector::_M_range_check: __n (which is 281473647845376) >= this->size() (which is 187650136359000) Aborted (core dumped)

Maybe I use the wrong vocab or framesets because I can run the demo well. But I can't figure it out. Could you offer more information for evaluation as chapter 5.5 in the paper? Thank you!

ForLoveandPeace commented 1 year ago

Hi, excuse me. The above is wrong input resulting the ERROR. Then I used the overcast-reference dataset as map on visuallocalization.net, and input the frames from No.500 to No.900(not all) in the RobotCar datasets 2014/12/16. The result is unacceptable sometimes, and the calibration time is too long (about 4000s). Lately, I input frames in the overcast-reference dataset, It got good result: camera1_rotation_error: 0.23585310974806373 deg, camera1_translation_error: 0.0777767603135562 m camera2_rotation_error: 0.27359521648537377 deg, camera2_translation_error: 0.0402563444762591 m

But it takes about 20000s. the log is below: # INFO: Calibration mode InRaSU # INFO: Using radial pose absolute estimator with unknown intrinsics. # INFO: Initializing... Get images from input directory # INFO: Number of frames: 148 # INFO: Loading map... Finished. # INFO: Setting up location recognition... Finished. # INFO: Initialization finished!

best guess camera_0 1.0000000000 0.0000000000 -0.0000000000 -0.0000000000 -0.0000000000 1.0000000000 -0.0000000000 -0.0000000000 0.0000000000 0.0000000000 1.0000000000 -0.0000000000

camera_1 -0.8542326506 -0.1393669547 0.5008626864 -7.4399780694 0.1003734501 0.9010628595 0.4219133723 -6.3292274200 -0.5101095462 0.4106854941 -0.7557285728 -8.4644054984

camera_2 -0.2909619554 -0.0267165863 -0.9563615240 5.4799453537 -0.2127551190 0.9763875190 0.0374522638 -0.2533501189 0.9327788590 0.2143679937 -0.2897757123 -18.1184868243

# INFO: Radial reprojection error: avg = 2.6870613288 px | max = 311.3938962912 px | count = 87661 after cam pose optimization, cam ref camera_0 0.1946049226 0.6223102897 -0.7581944522 -3.3596171299 -0.9352519465 -0.1152936415 -0.3346807028 4.4844391582 -0.2956902445 0.7742333495 0.5595801996 -1.8699514759

camera_1 0.2811408241 0.2252675545 0.9328528104 -0.0265837057 0.9585928053 -0.1118894869 -0.2618789346 0.9313786407 0.0453835951 0.9678508519 -0.2473965596 -1.0861393188

camera_2 -0.8978362643 0.4368950404 0.0548886711 -7.0790732266 -0.0162617649 -0.1574678861 0.9873902065 0.3453046741 0.4400290872 0.8856221477 0.1484850630 0.3206414663

# INFO: Radial reprojection error: avg = 2.3878298383 px | max = 267.1027713270 px | count = 87661 after cam pose BA, cam ref camera_0 0.1767040162 0.6194637572 -0.7648793004 -3.2356745606 -0.9402517487 -0.1235439063 -0.3172751995 4.4265164146 -0.2910366637 0.7752429018 0.5606211766 -1.9094473425

camera_1 0.2993697588 0.2728733334 0.9142854540 -0.2024548363 0.9526717222 -0.1385735406 -0.2705807892 0.8888084995 0.0528614905 0.9520176037 -0.3014434359 -1.4118602912

camera_2 -0.8976586162 0.4387054287 0.0417918118 -7.0829574582 -0.0205012147 -0.1363010578 0.9904553104 0.2430849605 0.4402143897 0.8882339604 0.1313458132 0.4206059857

# INFO: Radial reprojection error: avg = 0.6313277551 px | max = 306.0861156771 px | count = 87661 after cam upgrade camera_0 0.1767040162 0.6194637572 -0.7648793004 -7.6114368845 -0.9402517487 -0.1235439063 -0.3172751995 2.6114315201 -0.2910366637 0.7752429018 0.5606211766 1.2977842051

camera_1 0.2993697588 0.2728733334 0.9142854540 -6.4181246681 0.9526717222 -0.1385735406 -0.2705807892 2.7283224514 0.0528614905 0.9520176037 -0.3014434359 0.6374699945

camera_2 -0.8976586162 0.4387054287 0.0417918118 -6.9646606353 -0.0205012147 -0.1363010578 0.9904553104 3.0466895753 0.4402143897 0.8882339604 0.1313458132 0.7923963371

# INFO: Reprojection error: avg = 2285038293.8231816292 px | max = 200293852993023.8437500000 px | count = 87661 after cam upgrade BA camera_0 0.1767040162 0.6194637572 -0.7648793004 -7.5200347290 -0.9402517487 -0.1235439063 -0.3172751995 2.6493455245 -0.2910366637 0.7752429018 0.5606211766 1.2307906494

camera_1 0.2993697588 0.2728733334 0.9142854540 -6.2807005677 0.9526717222 -0.1385735406 -0.2705807892 2.6876520882 0.0528614905 0.9520176037 -0.3014434359 0.5921607393

camera_2 -0.8976586162 0.4387054287 0.0417918118 -6.9581610582 -0.0205012147 -0.1363010578 0.9904553104 3.2007278991 0.4402143897 0.8882339604 0.1313458132 0.8128235979

# INFO: Reprojection error: avg = 9284980.2253479902 px | max = 803905600266.3970947266 px | count = 87661 # INFO: Optimize camera poses, odometry. # INFO: Optimize camera intrinsics as well. # INFO: Reprojection error: avg = 9284980.2253479902 px | max = 803905600266.3970947266 px | count = 87661 # INFO: Optimization took 85.6704239845 s. # INFO: Reprojection error: avg = 5892733.2941709394 px | max = 509103871439.6717529297 px | count = 87661 # INFO: Initial reprojection error: avg = 5892733.2941709394 px | max = 509103871439.6717529297 px | count = 87661 # INFO: Reprojection error after pruning: avg = 1.1094530633 px | max = 19.9693894889 px | count = 60467 # INFO: Optimize camera poses, odometry. # INFO: Optimize camera intrinsics as well. # INFO: Reprojection error: avg = 1.1094530633 px | max = 19.9693894889 px | count = 60467 # INFO: Optimization took 154.5161156654 s. # INFO: Reprojection error: avg = 1.1354267896 px | max = 32.3333814542 px | count = 60467 # INFO: Odometry distance: 119.9738675803 m final result camera_0 0.1752034624 0.6205392328 -0.7643525412 -7.5213080070 -0.9407050624 -0.1235463139 -0.3159276719 2.6495728754 -0.2904784542 0.7743819270 0.5620986558 1.2328052177

camera_1 0.2975021648 0.2710294728 0.9154427818 -6.2771850479 0.9531995523 -0.1384350274 -0.2687868239 2.6872062237 0.0538801954 0.9525643118 -0.2995298925 0.5911738368

camera_2 -0.8977797296 0.4386056945 0.0402069884 -6.9584619515 -0.0216483029 -0.1351194507 0.9905927948 3.2025702995 0.4399123868 0.8884637184 0.1308025728 0.8094333570

# INFO: Calibration took 20558.0570540428 s.

Sorry to bother you. best wishes!

youkely commented 1 year ago

Thanks for your interest. Maybe you can record the time spent on each step. The output log shows that the optimization takes roughly 2-3 minutes which is OK. You can try to output the time spent on each step and see what happens. Also, maybe there is something wrong loading the map.

  1. Independent 1D radial pose estimation.
  2. Radial camera rig initialization.
  3. Radial Bundle Adjustment.
  4. Forward translation and intrinsic estimation.
  5. Final refinement.
ForLoveandPeace commented 1 year ago

Hi, thanks for you reply.

the function upgradeRadialCamera took 3194.3250665665 s.
It take most of the time.

Hi, Here is the log. # INFO: Calibration mode InRaSU # INFO: Using radial pose absolute estimator with unknown intrinsics. # INFO: Initializing... Get images from input directory # INFO: Number of frames: 22 # INFO: Loading map... Finished. # INFO: Setting up location recognition... Finished. # INFO: Initialization finished!

best guess camera_0 1.0000000000 0.0000000000 0.0000000000 0.0000000000 0.0000000000 1.0000000000 0.0000000000 0.0000000000 0.0000000000 0.0000000000 1.0000000000 0.0000000000

camera_1 -0.8533168913 -0.1467718271 0.5003082188 -8.8437494200 0.0901583104 0.9035702631 0.4188463425 -7.3775732680 -0.5135384719 0.4025156026 -0.7577990681 -9.9164414639

camera_2 -0.2847373876 -0.0343209020 -0.9579909685 6.3650277527 -0.2171305204 0.9756942620 0.0295811453 -0.2179761684 0.9336910395 0.2164319356 -0.2852687506 -21.3976717508

# INFO: Radial reprojection error: avg = 2.6139843502 px | max = 332.2275743420 px | count = 14316 # INFO: optimizeRadialRigpose took 0.2169010639 s. after cam pose optimization, cam ref camera_0 1.0000000000 0.0000000000 -0.0000000000 -0.0000000000 -0.0000000000 1.0000000000 0.0000000000 0.0000000000 0.0000000000 0.0000000000 1.0000000000 -0.0000000000

camera_1 -0.8448454040 -0.1412007860 0.5160412593 4.5505392729 0.1029428286 0.9036179530 0.4157852438 3.8178152867 -0.5250133497 0.4043969992 -0.7488818663 2.5534325863

camera_2 -0.2960701933 -0.0294522788 -0.9547120005 -1.8110421891 -0.2131612718 0.9763541300 0.0359845115 0.0493216154 0.9310771788 0.2141615655 -0.2953474412 8.8661055809

# INFO: Radial reprojection error: avg = 4.0842134849 px | max = 124.0866680944 px | count = 14316 # INFO: optimizeRadialRigposeBA took 15.5992279053 s. after cam pose BA, cam ref camera_0 1.0000000000 0.0000000000 -0.0000000000 0.0000000000 0.0000000000 1.0000000000 -0.0000000000 0.0000000000 -0.0000000000 0.0000000000 1.0000000000 -0.0000000000

camera_1 -0.8583907400 -0.0983596630 0.5034786135 4.6031311848 0.1094388139 0.9237422545 0.3670468542 3.3594202726 -0.5011870744 0.3701697231 -0.7821674326 2.3482978683

camera_2 -0.2688874983 -0.0521879804 -0.9617566885 -1.7397604433 -0.2139960122 0.9768106977 0.0068240490 -0.0311059918 0.9390980886 0.2076469976 -0.2738202045 8.9967694472

# INFO: Radial reprojection error: avg = 0.6306550475 px | max = 43.7367190065 px | count = 14316 # INFO: upgradeRadialCamera took 3194.3250665665 s. after cam upgrade camera_0 1.0000000000 -0.0000000000 0.0000000000 -0.0000000000 -0.0000000000 1.0000000000 -0.0000000000 0.0000000000 -0.0000000000 0.0000000000 1.0000000000 -0.0000000000

camera_1 -0.8583907400 -0.0983596630 0.5034786135 0.2461708479 0.1094388139 0.9237422545 0.3670468542 0.1831014721 -0.5011870744 0.3701697231 -0.7821674326 -0.9724169586

camera_2 -0.2688874983 -0.0521879804 -0.9617566885 -0.2140321055 -0.2139960122 0.9768106977 0.0068240490 -0.0419316456 0.9390980886 0.2076469976 -0.2738202045 -0.6582116105

# INFO: Reprojection error: avg = 6.1785411246 px | max = 5804.1418657699 px | count = 14316 # INFO: upgradeOptiRadialCamera took 15.5262880325 s. after cam upgrade BA camera_0 1.0000000000 -0.0000000000 -0.0000000000 0.0000000000 0.0000000000 1.0000000000 -0.0000000000 -0.0000000000 -0.0000000000 0.0000000000 1.0000000000 0.0000000000

camera_1 -0.8583907400 -0.0983596630 0.5034786135 0.3660431898 0.1094388139 0.9237422545 0.3670468542 0.2704910153 -0.5011870744 0.3701697231 -0.7821674326 -1.3172873598

camera_2 -0.2688874983 -0.0521879804 -0.9617566885 -0.3039084937 -0.2139960122 0.9768106977 0.0068240490 -0.0412939367 0.9390980886 0.2076469976 -0.2738202045 -0.8424456990

# INFO: Reprojection error: avg = 12.2966009757 px | max = 77394.2020439801 px | count = 14316 # INFO: Optimize camera poses, odometry. # INFO: Optimize camera intrinsics as well. # INFO: Reprojection error: avg = 12.2966009757 px | max = 77394.2020439801 px | count = 14316 # INFO: Optimization took 21.6202778816 s. # INFO: Reprojection error: avg = 11.9389659635 px | max = 75058.4259156985 px | count = 14316 # INFO: Initial reprojection error: avg = 11.9389659635 px | max = 75058.4259156985 px | count = 14316 # INFO: Reprojection error after pruning: avg = 1.1571695442 px | max = 19.8882252626 px | count = 11864 # INFO: upgradeOptiRadialCamera took 1.2966516018 s. # INFO: Optimize camera poses, odometry. # INFO: Optimize camera intrinsics as well. # INFO: Reprojection error: avg = 1.1571695442 px | max = 19.8882252626 px | count = 11864 # INFO: Optimization took 36.3198845387 s. # INFO: Reprojection error: avg = 1.1760785249 px | max = 30.2082508560 px | count = 11864 # INFO: Odometry distance: 15.3557552626 m final result camera_0 1.0000000000 -0.0000000000 -0.0000000000 0.0000000000 0.0000000000 1.0000000000 0.0000000000 0.0000000000 -0.0000000000 -0.0000000000 1.0000000000 0.0000000000

camera_1 -0.8577327399 -0.0995296325 0.5043693083 0.3702764711 0.1101299600 0.9227316976 0.3693746150 0.2691664718 -0.5021612678 0.3723708723 -0.7804959927 -1.3197103251

camera_2 -0.2695884710 -0.0551897685 -0.9613928155 -0.2988593103 -0.2146631521 0.9766794234 0.0041273449 -0.0513581694 0.9387447936 0.2074882968 -0.2751487220 -0.8438372498

# INFO: Calibration took 3389.8267490864 s.

youkely commented 1 year ago

I have never met this problem before. For camera upgrading we use solver from radialpose lib(https://github.com/vlarsson/radialpose). You may check the function on upgrading to see what is happening (https://github.com/youkely/InfrasCal/blob/master/src/infrastr_calib/InfrastructureCalibration.cc#L1459). For example, output the time spent for solving one minimal problem.

Since it use minimal solver which requires only 5 correspondences, it should be quite quick to solve. You might also check if there are too many loops inside this function.

ForLoveandPeace commented 1 year ago

I found the RANSAC takes the most of time! I add code to get the time in the loop(https://github.com/youkely/InfrasCal/blob/master/src/infrastr_calib/InfrastructureCalibration.cc#L1560)


for (int j = 0; j < poses.size(); j++) {
    std::vector<Point2DFeaturePtr> inliersIds;
    Eigen::Matrix4d T_ref_cam = T_cam_ref.at(camid).toMatrix().inverse();
    T_ref_cam(2,3)=poses[j].t(2);

    double tsStart_ = timeInSeconds();

    for (size_t k = 0; k < indices.size(); ++k) {
        //transform X to camera frame with cam_T_ref and ref_T_world
        const FramePtr& frame = indices.at(k)->frame().lock();
        Eigen::Matrix4d T_world_ref = frame->systemPose()->toMatrix().inverse();
        Eigen::Matrix4d T_world_cam = T_ref_cam * T_world_ref;

        Eigen::Vector3d P2 = indices.at(k)->feature3D()->point();
        Eigen::Vector3d P1 = transformPoint(T_world_cam, P2);
        Eigen::Vector2d p1_pred(P1[0] / P1[2], P1[1] / P1[2]);
        Eigen::Matrix<double, 2, Eigen::Dynamic> p1_pred_;
        p1_pred_.resize(2, 1);
        p1_pred_.block<2, 1>(0, 0) = p1_pred;

        if (camera1->modelType() == Camera::PINHOLE)
            radialpose::forward_rational_model(poses[j].dist_params, 2, 0, p1_pred_, &p1_pred_);
        else if (camera1->modelType() == Camera::KANNALA_BRANDT)
            radialpose::inverse_rational_model(poses[j].dist_params, 0, 2, p1_pred_, &p1_pred_);
        else
            radialpose::forward_rational_model(poses[j].dist_params, 2, 0, p1_pred_, &p1_pred_);

        p1_pred = p1_pred_.block<2, 1>(0, 0);
        p1_pred(0) = p1_pred(0) * poses[j].focal + cxcy[0];
        p1_pred(1) = p1_pred(1) * poses[j].focal + cxcy[1];
        const Point2DFeatureConstPtr &f1 = indices.at(k);

        double err = hypot(f1->keypoint().pt.x - p1_pred(0),
                            f1->keypoint().pt.y - p1_pred(1));
        if (!isnormal(err) || err > k_reprojErrorThresh*5) {
            continue;
        }

        inliersIds.push_back(indices.at(k));
    }
    std::cout << "# INFO: ransac took "
        << timeInSeconds() - tsStart_ << " s." << std::endl;
    if (inliersIds.size() > inlierIds_best.size()) {
        cam_param_best = poses[j];
        T_ref_cam_best = T_ref_cam;
        inlierIds_best = inliersIds;

    }
    std::cout << "# INFO: upgradeRadialCamera once in the ransac took "
                  << timeInSeconds() - tsStart << " s." << std::endl;
}

Here is the output: # INFO: ransac took 0.7256283760 s. # INFO: ransac took 0.7203974724 s. # INFO: ransac took 0.7203898430 s. # INFO: ransac took 0.7195880413 s. # INFO: ransac took 0.7196631432 s. # INFO: ransac took 0.7199099064 s. # INFO: ransac took 0.7197566032 s. # INFO: upgradeRadialCamera once in the ransac took 5.0781514645 s.

The all time can be calculated: 5s 3(cam number) N(252) = 3780s. Not all times takes 5s due to the poses got from the minimal solver whose size is variable. Maybe it is due to too many feature_2d( 14316 ) that it takes 5s to get all inliers. Thank you for reply.

ForLoveandPeace commented 1 year ago

Hi, I let the RANSAC iteration end if inliers are enough and change some pamameters in InfrastureCalibration. Now it takes an acceptable time to calibrate camera rig(it takes 800s when i input 200 frames per cam). Thanks for your great work!

And is there any possibility to calibrate camera model MEI by changing the code? I am not sure if the theory in the paper applies to this model. In most scenarios, camera rig contains cameras which are different models. So I want to do some extentions. Thanks!

youkely commented 1 year ago

Great. For the camera model MEI, it uses quite different omni-directional model rather pinhole. So essentially the intrinsics is rather different. The main problem is that whether you can find a good way to adapt the camera upgrading step to MEI model, the other steps should be OK.