search

opencv / opencv

Open Source Computer Vision Library
https://opencv.org
Apache License 2.0
75.69k stars 55.61k forks source link

calibrateHandEye does not work when inputting CV_32F Rodriguess matrices #25421

Closed LuukvandenBent closed 1 week ago

LuukvandenBent commented 2 weeks ago

System Information

OpenCV version: 4.8.1 OS: Windows 11 Compiler: MSVC 2022

Detailed description

When calling calibrateHandEye function the inputs are required to be in format vector Mat (3x3) rotation matrices or (3x1) rotation vectors (Rodrigues) according to the docs. Internally these are transformed into homogeneous transforms of type CV_64F. When input is of format (3x1)/Rodrigues the Rodrigues function is used to convert to (3x3) rotation matrix. However, this function supports both CV_32F and CV_64F inputs and does not recast. This results in the fact that the resulting (3x3) rotation matrix is not in CV_64F format when input is CV_32F, which results in the function not working correctly. 

    std::vector<Mat> Hg;
    Hg.reserve(R_gripper2base_.size());
    for (size_t i = 0; i < R_gripper2base_.size(); i++)
    {
        Mat m = Mat::eye(4, 4, CV_64FC1);
        Mat R = m(Rect(0, 0, 3, 3));
        if(R_gripper2base_[i].size() == Size(3, 3))
            R_gripper2base_[i].convertTo(R, CV_64F);
        else
            Rodrigues(R_gripper2base_[i], R);

        Mat t = m(Rect(3, 0, 1, 3));
        t_gripper2base_[i].convertTo(t, CV_64F);

        Hg.push_back(m);
    }

Steps to reproduce

#include <opencv2/core.hpp>
#include <opencv2/calib3d.hpp>
#include <opencv2/opencv.hpp>
#include <iostream>
#include <vector>
#include <string>

inline cv::Mat MatTo64F(const cv::Mat& mat) {
    cv::Mat matDouble;
    mat.convertTo(matDouble, CV_64F);
    return matDouble;
}

inline std::vector<cv::Mat> VectorMatTo64F(const std::vector<cv::Mat>& vectorMat) {
    std::vector<cv::Mat> vectorMat64F;
    for (const auto& mat : vectorMat) {
        cv::Mat matDouble = MatTo64F(mat);
        vectorMat64F.push_back(matDouble);
    }
    return vectorMat64F;
}

int main()
{
    std::vector<cv::Mat> R_endEffector2Base;
    R_endEffector2Base.push_back((cv::Mat_<float>(3, 1) << 2.221440553665161, -2.221440553665161, 3.359327365615172e-06));
    R_endEffector2Base.push_back((cv::Mat_<float>(3, 1) << -2.783314943313599, 1.456495523452759, -0.0002249035169370472));
    R_endEffector2Base.push_back((cv::Mat_<float>(3, 1) << -3.140150785446167, 0.06004027277231216, 1.706913826637901e-05));
    R_endEffector2Base.push_back((cv::Mat_<float>(3, 1) << -3.140150785446167, 0.06004027277231216, 1.706913826637901e-05));
    R_endEffector2Base.push_back((cv::Mat_<float>(3, 1) << -3.139387369155884, 0.09073258936405182, 1.882561446109321e-05));
    R_endEffector2Base.push_back((cv::Mat_<float>(3, 1) << 2.221440553665161, -2.221440553665161, 3.359327365615172e-06));
    R_endEffector2Base.push_back((cv::Mat_<float>(3, 1) << -2.783314943313599, 1.456495523452759, -0.0002249035169370472));
    R_endEffector2Base.push_back((cv::Mat_<float>(3, 1) << -3.140150785446167, 0.06004027277231216, 1.706913826637901e-05));
    R_endEffector2Base.push_back((cv::Mat_<float>(3, 1) << -3.023658990859985, 0.05816467106342316, -0.8101729154586792));
    R_endEffector2Base.push_back((cv::Mat_<float>(3, 1) << -2.672714710235596, 0.05111135914921761, -0.0121563570573926));
    R_endEffector2Base.push_back((cv::Mat_<float>(3, 1) << 2.221440553665161, -2.221440553665161, 3.359327365615172e-06));
    R_endEffector2Base.push_back((cv::Mat_<float>(3, 1) << -2.783314943313599, 1.456495523452759, -0.0002249035169370472));
    R_endEffector2Base.push_back((cv::Mat_<float>(3, 1) << -3.140150785446167, 0.06004027277231216, 1.706913826637901e-05));
    R_endEffector2Base.push_back((cv::Mat_<float>(3, 1) << -3.023658990859985, 0.05816467106342316, -0.8101729154586792));
    R_endEffector2Base.push_back((cv::Mat_<float>(3, 1) << -2.672714710235596, 0.05111135914921761, -0.0121563570573926));
    R_endEffector2Base.push_back((cv::Mat_<float>(3, 1) << 2.221440553665161, -2.221440553665161, 3.359327365615172e-06));
    R_endEffector2Base.push_back((cv::Mat_<float>(3, 1) << -2.783314943313599, 1.456495523452759, -0.0002249035169370472));
    R_endEffector2Base.push_back((cv::Mat_<float>(3, 1) << -3.140150785446167, 0.06004027277231216, 1.706913826637901e-05));
    R_endEffector2Base.push_back((cv::Mat_<float>(3, 1) << -3.023658990859985, 0.05816467106342316, -0.8101729154586792));
    R_endEffector2Base.push_back((cv::Mat_<float>(3, 1) << -2.672715187072754, 0.05111018940806389, -0.01215619966387749));
    R_endEffector2Base.push_back((cv::Mat_<float>(3, 1) << 2.221440553665161, -2.221440553665161, 3.359327365615172e-06));
    R_endEffector2Base.push_back((cv::Mat_<float>(3, 1) << -2.783314943313599, 1.456495523452759, -0.0002249035169370472));
    R_endEffector2Base.push_back((cv::Mat_<float>(3, 1) << -3.140150785446167, 0.06004027277231216, 1.706913826637901e-05));
    R_endEffector2Base.push_back((cv::Mat_<float>(3, 1) << -3.023658990859985, 0.05816458165645599, -0.8101732730865479));
    R_endEffector2Base.push_back((cv::Mat_<float>(3, 1) << -2.672714948654175, 0.05111094936728477, -0.01215606927871704));

    std::vector<cv::Mat> t_endEffector2Base;
    t_endEffector2Base.push_back((cv::Mat_<float>(3, 1) << -0.2999995052814484, 1.749998092651367, -0.8500044345855713));
    t_endEffector2Base.push_back((cv::Mat_<float>(3, 1) << -1.519304633140564, 1.241297245025635, -0.8497245907783508));
    t_endEffector2Base.push_back((cv::Mat_<float>(3, 1) << -2.456427335739136, -0.6568424701690674, -0.8505849838256836));
    t_endEffector2Base.push_back((cv::Mat_<float>(3, 1) << -2.481236219406128, -1.305367588996887, -0.8511480093002319));
    t_endEffector2Base.push_back((cv::Mat_<float>(3, 1) << -1.571397185325623, -0.4146167039871216, -0.8437739014625549));
    t_endEffector2Base.push_back((cv::Mat_<float>(3, 1) << -0.2999998033046722, 1.749999284744263, -0.3500044643878937));
    t_endEffector2Base.push_back((cv::Mat_<float>(3, 1) << -1.519419074058533, 1.241232872009277, -0.3497245907783508));
    t_endEffector2Base.push_back((cv::Mat_<float>(3, 1) << -2.456430196762085, -0.657276451587677, -0.3505851626396179));
    t_endEffector2Base.push_back((cv::Mat_<float>(3, 1) << -1.968876242637634, -1.155877232551575, -1.541896820068359));
    t_endEffector2Base.push_back((cv::Mat_<float>(3, 1) << -1.777414560317993, -0.4138471782207489, -0.3607091903686523));
    t_endEffector2Base.push_back((cv::Mat_<float>(3, 1) << -0.3000001013278961, 1.750000476837158, 0.1499955803155899));
    t_endEffector2Base.push_back((cv::Mat_<float>(3, 1) << -1.519533514976501, 1.241168618202209, 0.1503482460975647));
    t_endEffector2Base.push_back((cv::Mat_<float>(3, 1) << -2.456433057785034, -0.6577104330062866, 0.1494155526161194));
    t_endEffector2Base.push_back((cv::Mat_<float>(3, 1) << -2.218879222869873, -1.156253099441528, -1.108885049819946));
    t_endEffector2Base.push_back((cv::Mat_<float>(3, 1) << -1.786186933517456, -0.6434944868087769, 0.09345653653144836));
    t_endEffector2Base.push_back((cv::Mat_<float>(3, 1) << -0.30000039935112, 1.750001668930054, 0.6499956250190735));
    t_endEffector2Base.push_back((cv::Mat_<float>(3, 1) << -1.51964795589447, 1.241104245185852, 0.6503482460975647));
    t_endEffector2Base.push_back((cv::Mat_<float>(3, 1) << -2.456435918807983, -0.6581444144248962, 0.6494153738021851));
    t_endEffector2Base.push_back((cv::Mat_<float>(3, 1) << -2.468881607055664, -1.156628847122192, -0.675873875617981));
    t_endEffector2Base.push_back((cv::Mat_<float>(3, 1) << -2.122421264648438, -0.8565499186515808, 0.5396002531051636));
    t_endEffector2Base.push_back((cv::Mat_<float>(3, 1) << -0.3000006973743439, 1.750002861022949, 1.149995684623718));
    t_endEffector2Base.push_back((cv::Mat_<float>(3, 1) << -1.519762396812439, 1.241039872169495, 1.15034818649292));
    t_endEffector2Base.push_back((cv::Mat_<float>(3, 1) << -2.456438779830933, -0.6585784554481506, 1.149415254592896));
    t_endEffector2Base.push_back((cv::Mat_<float>(3, 1) << -2.706493377685547, -0.7829992175102234, -0.235385000705719));
    t_endEffector2Base.push_back((cv::Mat_<float>(3, 1) << -2.473471164703369, -0.9334863424301147, 0.7180644273757935));

    std::vector<cv::Mat> R_camera2Board;
    R_camera2Board.push_back((cv::Mat_<float>(3, 1) << 0.0294183325022459, -0.04008607193827629, 2.494235754013062));
    R_camera2Board.push_back((cv::Mat_<float>(3, 1) << 0.04194911941885948, -0.02422361634671688, 1.887918591499329));
    R_camera2Board.push_back((cv::Mat_<float>(3, 1) << 0.04641126096248627, -0.01218053977936506, 0.9621399641036987));
    R_camera2Board.push_back((cv::Mat_<float>(3, 1) << 0.05417881906032562, -0.01331218238919973, 0.962380051612854));
    R_camera2Board.push_back((cv::Mat_<float>(3, 1) << 0.05594902485609055, -0.0125244427472353, 0.9819424152374268));
    R_camera2Board.push_back((cv::Mat_<float>(3, 1) << 0.03159823268651962, -0.04207834973931313, 2.494164943695068));
    R_camera2Board.push_back((cv::Mat_<float>(3, 1) << 0.04239876568317413, -0.02777188457548618, 1.888404846191406));
    R_camera2Board.push_back((cv::Mat_<float>(3, 1) << 0.05072414129972458, -0.01285374350845814, 0.9621807932853699));
    R_camera2Board.push_back((cv::Mat_<float>(3, 1) << 0.53544020652771, 0.2373146116733551, 0.9455408453941345));
    R_camera2Board.push_back((cv::Mat_<float>(3, 1) << -0.1451666355133057, 0.4248732328414917, 0.9515339136123657));
    R_camera2Board.push_back((cv::Mat_<float>(3, 1) << 0.03051971644163132, -0.04057196900248528, 2.494425058364868));
    R_camera2Board.push_back((cv::Mat_<float>(3, 1) << 0.04452834650874138, -0.02820436656475067, 1.888351917266846));
    R_camera2Board.push_back((cv::Mat_<float>(3, 1) << 0.05279968306422234, -0.01134111545979977, 0.9623869061470032));
    R_camera2Board.push_back((cv::Mat_<float>(3, 1) << 0.5378463864326477, 0.2360504567623138, 0.9454251527786255));
    R_camera2Board.push_back((cv::Mat_<float>(3, 1) << -0.1483558118343353, 0.4276632964611053, 0.9511608481407166));
    R_camera2Board.push_back((cv::Mat_<float>(3, 1) << 0.03274118900299072, -0.03915755823254585, 2.494407653808594));
    R_camera2Board.push_back((cv::Mat_<float>(3, 1) << 0.04386329278349876, -0.02741651609539986, 1.888501405715942));
    R_camera2Board.push_back((cv::Mat_<float>(3, 1) << 0.05244381725788116, -0.01107311807572842, 0.9627002477645874));
    R_camera2Board.push_back((cv::Mat_<float>(3, 1) << 0.5366753339767456, 0.2356787621974945, 0.9455472826957703));
    R_camera2Board.push_back((cv::Mat_<float>(3, 1) << -0.1477671414613724, 0.428054541349411, 0.9507550001144409));
    R_camera2Board.push_back((cv::Mat_<float>(3, 1) << 0.03280184790492058, -0.04059437289834023, 2.494607210159302));
    R_camera2Board.push_back((cv::Mat_<float>(3, 1) << 0.04482218623161316, -0.02734432928264141, 1.888676047325134));
    R_camera2Board.push_back((cv::Mat_<float>(3, 1) << 0.05307869613170624, -0.01116583216935396, 0.9629517793655396));
    R_camera2Board.push_back((cv::Mat_<float>(3, 1) << 0.5375314354896545, 0.2368061691522598, 0.9453787207603455));
    R_camera2Board.push_back((cv::Mat_<float>(3, 1) << -0.1507342904806137, 0.4287057220935822, 0.9507918357849121));

    std::vector<cv::Mat> t_camera2Board;
    t_camera2Board.push_back((cv::Mat_<float>(3, 1) << 0.5406627655029297, 0.2993817627429962, 4.028430938720703));
    t_camera2Board.push_back((cv::Mat_<float>(3, 1) << 0.551082968711853, -0.114240325987339, 4.009007453918457));
    t_camera2Board.push_back((cv::Mat_<float>(3, 1) << 0.3056039810180664, -0.5887037515640259, 3.993506669998169));
    t_camera2Board.push_back((cv::Mat_<float>(3, 1) << 0.700834333896637, -0.07520329207181931, 4.014020442962646));
    t_camera2Board.push_back((cv::Mat_<float>(3, 1) << -0.5247818231582642, -0.2456959336996078, 3.999956846237183));
    t_camera2Board.push_back((cv::Mat_<float>(3, 1) << 0.5386375188827515, 0.3221721947193146, 3.529111623764038));
    t_camera2Board.push_back((cv::Mat_<float>(3, 1) << 0.5495628118515015, -0.09137704223394394, 3.510187864303589));
    t_camera2Board.push_back((cv::Mat_<float>(3, 1) << 0.3037885725498199, -0.5659011006355286, 3.493125915527344));
    t_camera2Board.push_back((cv::Mat_<float>(3, 1) << 0.6397021412849426, -0.1812979280948639, 3.241022109985352));
    t_camera2Board.push_back((cv::Mat_<float>(3, 1) << -0.2120662033557892, -0.1457731574773788, 3.609678506851196));
    t_camera2Board.push_back((cv::Mat_<float>(3, 1) << 0.5365630388259888, 0.3450987935066223, 3.030184030532837));
    t_camera2Board.push_back((cv::Mat_<float>(3, 1) << 0.5474436283111572, -0.06856922805309296, 3.010627031326294));
    t_camera2Board.push_back((cv::Mat_<float>(3, 1) << 0.3019931614398956, -0.5431217551231384, 2.993245363235474));
    t_camera2Board.push_back((cv::Mat_<float>(3, 1) << 0.6379250288009644, -0.1581491678953171, 2.741015672683716));
    t_camera2Board.push_back((cv::Mat_<float>(3, 1) << -0.2142543941736221, -0.1219313442707062, 3.103338003158569));
    t_camera2Board.push_back((cv::Mat_<float>(3, 1) << 0.5343272089958191, 0.3682735860347748, 2.530436754226685));
    t_camera2Board.push_back((cv::Mat_<float>(3, 1) << 0.5455394387245178, -0.04546698182821274, 2.511759281158447));
    t_camera2Board.push_back((cv::Mat_<float>(3, 1) << 0.300304114818573, -0.5205106735229492, 2.494507551193237));
    t_camera2Board.push_back((cv::Mat_<float>(3, 1) << 0.6360357999801636, -0.1354428976774216, 2.242185831069946));
    t_camera2Board.push_back((cv::Mat_<float>(3, 1) << 0.04323123767971992, -0.2985694408416748, 2.594571590423584));
    t_camera2Board.push_back((cv::Mat_<float>(3, 1) << 0.5324299335479736, 0.3913149535655975, 2.031616449356079));
    t_camera2Board.push_back((cv::Mat_<float>(3, 1) << 0.5434697866439819, -0.02255551889538765, 2.012497425079346));
    t_camera2Board.push_back((cv::Mat_<float>(3, 1) << 0.2988114356994629, -0.4978908896446228, 1.995307326316833));
    t_camera2Board.push_back((cv::Mat_<float>(3, 1) << 0.4059097766876221, -0.4089950323104858, 1.72959291934967));
    t_camera2Board.push_back((cv::Mat_<float>(3, 1) << 0.3187371492385864, -0.5010331273078918, 2.385546445846558));

    cv::Mat R_base2Camera, t_base2Camera;

    cv::HandEyeCalibrationMethod handEyeMethod = cv::CALIB_HAND_EYE_PARK;
    cv::calibrateHandEye(R_endEffector2Base, t_endEffector2Base, R_camera2Board, t_camera2Board, R_base2Camera, t_base2Camera, handEyeMethod);

    std::cout << R_base2Camera << std::endl;
    std::cout << t_base2Camera << std::endl;

    cv::calibrateHandEye(VectorMatTo64F(R_endEffector2Base), t_endEffector2Base, VectorMatTo64F(R_camera2Board), t_camera2Board, R_base2Camera, t_base2Camera, handEyeMethod);

    std::cout << R_base2Camera << std::endl;
    std::cout << t_base2Camera << std::endl;
}

output:

[-nan(ind), -nan(ind), -nan(ind);
 -nan(ind), -nan(ind), -nan(ind);
 -nan(ind), -nan(ind), -nan(ind)]
[0;0;0]

[0.7913096619242659, -0.6106482221127517, -0.03062299423371861;
 -0.6113964228983049, -0.7906882909948714, -0.03172444720982304;
 -0.004840765689617499, 0.04382665072927122, -0.9990274228835736]
[2.465654425645377; -0.08552923770120993;2.945520276585262]

Issue submission checklist