P1P2LL

[Armin234]

Hi,

I have one 2d - 3D correspondence
(0 / -0.1 / 1 ) -> (10 / 50 / 100)

and two line 2D - 3D correspondence

(0 / -0.1 / 1) + a * (1 / 0 / 0) - > (0 / 10 / 100) + a * (1 / 0 / 0)
(0 / 0.1 / 1) + a * (1 / 0 / 0) - > (0 / 30 / 110) + a * (1 / 0.02 / 0.01)

can everyone help me to solve the problem, or help me
to initialize the parameters for the function p1p2ll ?

thanks, regards
Armin

[Viktor Larsson]

Hi,
for the p1p2ll function the 2D lines need to be in the homogeneous representation, instead of point + direction. See the code below

// the point correspondence
Eigen::Vector3d xp(0.0, -0.1, 1.0);
Eigen::Vector3d Xp(10.0, 50.0, 100.0);

// bearing vector for point needs to be normalized!
xp.normalize();
    
// and two line 2D - 3D correspondence
Eigen::Vector3d x1(0.0, -0.1, 1.0);
Eigen::Vector3d v1(1.0, 0.0, 0.0);
Eigen::Vector3d x2(0.0, 0.1, 1.0);
Eigen::Vector3d v2(1.0, 0.0, 0.0);

// we convert the 2d lines to homogeneous representation
Eigen::Vector3d l1 = x1.cross(v1);
Eigen::Vector3d l2 = x2.cross(v2);

// 3D lines
Eigen::Vector3d X1(0.0, 10.0, 100.0);
Eigen::Vector3d V1(1.0, 0.0, 0.0);
Eigen::Vector3d X2(0.0, 30.0, 110.0);    
Eigen::Vector3d V2(1.0, 0.02, 0.01);    

std::vector<pose_lib::CameraPose> poses;

int n_sols = pose_lib::p1p2ll({ xp }, { Xp }, {l1, l2}, { X1, X2 }, { V1, V2 }, &poses);

for (pose_lib::CameraPose& pose : poses) {
    std::cout << "R = \n" << pose.R << "\n t = " << pose.t.transpose() << "\n";

    double err_p = (xp.hnormalized() - (pose.R * Xp + pose.t).hnormalized()).norm();

    double err_l1_p = std::abs(l1.dot(pose.R * X1 + pose.t));
    double err_l1_v = std::abs(l1.dot(pose.R * V1));
    double err_l2_p = std::abs(l2.dot(pose.R * X2 + pose.t));
    double err_l2_v = std::abs(l2.dot(pose.R * V2));

    std::cout << "err = " << err_p << " "
                << err_l1_p << " " << err_l1_v << " "
                << err_l2_p << " " << err_l2_v << "\n";
}

For this particular instance though something weird seems to be happening where it does not satisfy all of the input constraints. (All are errors are zero except err_l2_p in the code above.) Do you know if this instance should have real solutions (i.e. do you know the "correct" pose?)

[Armin234]

Hi Larsson,

thank you for the example, you are right the instance didn't have real solutions, I had some error inside my data.

Regards
Armin