Skip to content
Snippets Groups Projects
Commit 238d0b8c authored by Angel Santamaria-Navarro's avatar Angel Santamaria-Navarro
Browse files

WIP trifocal tensor trilinearities. only the first one missing

parent 98696ff2
No related branches found
No related tags found
No related merge requests found
......@@ -196,19 +196,19 @@ bool TrifocalTensor::computeTensorFromProjectionMat(const Eigen::MatrixXd& P1, c
Eigen::Matrix3d wRc1 = P1.block(0,0,3,3);
Eigen::Matrix3d wRc2 = P2.block(0,0,3,3);
Eigen::Matrix3d wRc3 = P3.block(0,0,3,3);
Eigen::Vector3d wtc1 = P3.block(0,3,3,1);
Eigen::Vector3d wtc2 = P3.block(0,3,3,1);
Eigen::Vector3d wtc1 = P1.block(0,3,3,1);
Eigen::Vector3d wtc2 = P2.block(0,3,3,1);
Eigen::Vector3d wtc3 = P3.block(0,3,3,1);
Eigen::Matrix3d c1Rc2 = wRc1.transpose()*wRc2;
Eigen::Matrix3d c1Rc3 = wRc1.transpose()*wRc3;
Eigen::Vector3d c1tc2 = -wRc1.transpose()*(wtc2-wtc1);
Eigen::Vector3d c1tc3 = -wRc1.transpose()*(wtc3-wtc1);
Eigen::Vector3d c1tc2 = wRc1.transpose()*(wtc2-wtc1);
Eigen::Vector3d c1tc3 = wRc1.transpose()*(wtc3-wtc1);
Eigen::MatrixXd c2Pc1(3,4), c3Pc1(3,4);
c2Pc1.block(0,0,3,3) = -c1Rc2.transpose();
c2Pc1.block(0,0,3,3) = c1Rc2.transpose();
c2Pc1.block(0,3,3,1) = -c1Rc2.transpose()*c1tc2;
c3Pc1.block(0,0,3,3) = -c1Rc3.transpose();
c3Pc1.block(0,0,3,3) = c1Rc3.transpose();
c3Pc1.block(0,3,3,1) = -c1Rc3.transpose()*c1tc3;
// Trifocal tensor from Projection matrices (c1->c2 and c1->c3)
......
......@@ -525,104 +525,101 @@ TEST(TrifocalTensor, ComputeTensorReal)
#endif
}
TEST(TrifocalTensor, computeTensorFromProjectionMat)
{
// Point in three images
// Point in three images (normalized 3D coordinates, not projected)
Eigen::Vector3d p1c1, p1c2, p1c3;
p1c1 << 0.0,0.0,1.0;
p1c2 << 0.1045, 0.0, 1.0;
p1c2 << 0.104464721937194, 0.0, 1.0;
p1c3 << -0.2,0.0,1.0;
VU_DEBUG("");
//Extrisinsics
Eigen::Vector3d wtc1, wtc2, wtc3;
wtc1 << 0,0,5;
wtc2 << 5,0,5;
wtc3 << 10,0,5;
VU_DEBUG("");
Eigen::Vector3d angc1, angc2, angc3;
angc1 << -90.0,45.0,0.0;
angc1 << -90.0,0.0,-45.0;
angc1 = angc1*M_PI/180.0;
// angc2 << -90.0,-20.0,0.0;
// angc2 = angc2*M_PI/180.0;
//
// angc3 << -90.0,-45.0,0.0;
// angc3 = angc3*M_PI/180.0;
VU_DEBUG("");
angc2 << -90.0,0.0,20.0;
angc2 = angc2*M_PI/180.0;
angc3 << -90.0,0.0,45.0;
angc3 = angc3*M_PI/180.0;
Eigen::Matrix3d wRc1 = vision_utils::matrixRollPitchYaw(angc1(0),angc1(1),angc1(2));
// Eigen::Matrix3d wRc2 = vision_utils::matrixRollPitchYaw(angc2(0),angc2(1),angc2(2));
// Eigen::Matrix3d wRc3 = vision_utils::matrixRollPitchYaw(angc3(0),angc3(1),angc3(2));
std::cout << " wRc1 " << std::endl << wRc1.transpose() << std::endl;
// std::cout << " wRc2 " << wRc2 << std::endl;
// std::cout << " wRc3 " << wRc3 << std::endl;
//
// VU_DEBUG("");
//
// // Extrinsics between cameras
// Eigen::Matrix3d c1Rc2 = wRc1.transpose()*wRc2;
// Eigen::Matrix3d c1Rc3 = wRc1.transpose()*wRc3;
// Eigen::Vector3d c1tc2 = -wRc1.transpose()*(wtc2-wtc1);
// Eigen::Vector3d c1tc3 = -wRc1.transpose()*(wtc3-wtc1);
//
// VU_DEBUG("");
//
// // Projection matrices (without K intrinsics)
// Eigen::MatrixXd P1(3,4), P2(3,4), P3(3,4);
// P1.block(0,0,3,3) = wRc1;
// P1.block(0,3,3,1) = wtc1;
// P2.block(0,0,3,3) = wRc2;
// P2.block(0,3,3,1) = wtc2;
// P3.block(0,0,3,3) = wRc3;
// P3.block(0,3,3,1) = wtc3;
Eigen::Matrix3d wRc2 = vision_utils::matrixRollPitchYaw(angc2(0),angc2(1),angc2(2));
Eigen::Matrix3d wRc3 = vision_utils::matrixRollPitchYaw(angc3(0),angc3(1),angc3(2));
// Extrinsics between cameras
Eigen::Matrix3d c1Rc2 = wRc1.transpose()*wRc2;
Eigen::Matrix3d c1Rc3 = wRc1.transpose()*wRc3;
Eigen::Vector3d c1tc2 = wRc1.transpose()*(wtc2-wtc1);
Eigen::Vector3d c1tc3 = wRc1.transpose()*(wtc3-wtc1);
// Projection matrices (without K intrinsics)
Eigen::MatrixXd P1(3,4), P2(3,4), P3(3,4);
P1.block(0,0,3,3) = wRc1;
P1.block(0,3,3,1) = wtc1;
P2.block(0,0,3,3) = wRc2;
P2.block(0,3,3,1) = wtc2;
P3.block(0,0,3,3) = wRc3;
P3.block(0,3,3,1) = wtc3;
// Compute tensor
vision_utils::TrifocalTensor tensor(P1,P2,P3);
// Essential Matrices
Eigen::Matrix3d c2Ec1 = c1Rc2.transpose()*vision_utils::skew(c1tc2);
Eigen::Matrix3d c3Ec1 = c1Rc3.transpose()*vision_utils::skew(c1tc3);
// Epipolar lines c1->c2 and c2->c3 in normalized C2 and C3 3D spaces
Eigen::Vector3d epLinec2_norm = c2Ec1*p1c1;
Eigen::Vector3d epLinec3_norm = c3Ec1*p1c1;
// Create 2D lines (perpendicular to epipolar lines)
Eigen::MatrixXd l1(3,1), l2(3,1), l3(3,1);
l1 = p1c1;
l2 << epLinec2_norm(1), -epLinec2_norm(0), -p1c2(0)*epLinec2_norm(1)+p1c2(1)*epLinec2_norm(0);
l3 << epLinec3_norm(1), -epLinec3_norm(0), -p1c3(0)*epLinec3_norm(1)+p1c3(1)*epLinec3_norm(0);
// Element computed using the tensor
Eigen::Matrix3d T1, T2, T3;
tensor.getLayers(T1,T2,T3);
Eigen::Matrix3d xTi(3,3);
xTi.col(0) = T1*p1c1;
xTi.col(1) = T2*p1c1;
xTi.col(2) = T3*p1c1;
// Verify trilinearities
// Line-line-line
// Eigen::MatrixXd lll = l2.transpose()*T1*T2*T3*l3*vision_utils::skew(l1);
// Eigen::MatrixXd lll = (l2.transpose()*T1*T2*T3)*l3;
// std::cout << lll << std::endl;
// % Line-line-line
// assert(all(all(l2'*tensor(:,:,1)*tensor(:,:,2)*tensor(:,:,3)*l3*skew(l1)==0)),'Trilinearity not accomplished');
//
// VU_DEBUG("");
//
// // Compute tensor
// vision_utils::TrifocalTensor tensor(P1,P2,P3);
//
// VU_DEBUG("");
//
// // Fundamental Matrices
// Eigen::Matrix3d F12 = c1Rc2.transpose()*vision_utils::skew(c1tc2);
// Eigen::Matrix3d F13 = c1Rc3.transpose()*vision_utils::skew(c1tc3);
//
// VU_DEBUG("");
//
// // Epipolar lines c1->c2 and c2->c3 in normalized C2 and C3 3D spaces
// Eigen::Vector3d epLinec2_norm = F12*p1c1;
// Eigen::Vector3d epLinec3_norm = F13*p1c1;
//
// VU_DEBUG("");
//
// // Create 2D lines (perpendicular to epipolar lines)
// Eigen::Vector3d l1, l2, l3;
// l1 = p1c1;
// l2 << epLinec2_norm(1), -epLinec2_norm(0), epLinec2_norm(0)*p1c2(1)-epLinec2_norm(1)*p1c2(0);
// l3 << epLinec3_norm(1), -epLinec3_norm(0), epLinec3_norm(0)*p1c3(1)-epLinec3_norm(1)*p1c3(0);
//
// VU_DEBUG("");
//
// // Element computed using the tensor
// Eigen::Matrix3d T1, T2, T3;
// tensor.getLayers(T1,T2,T3);
// Eigen::Matrix3d xTi(3,3);
// xTi = p1c1(0,0)*T1;
// xTi = xTi + p1c1(1,0)*T2;
// xTi = xTi + p1c1(2,0)*T3;
//
// std::cout << xTi << std::endl;
// Point-line-line
Eigen::MatrixXd pll = l2.transpose()*xTi*l3;
ASSERT_TRUE(pll(0,0)<1e-5);
// Point-line-point
Eigen::MatrixXd plp = l2.transpose()*xTi*vision_utils::skew(p1c3);
ASSERT_TRUE(plp(0,0)<1e-5 && plp(0,1)<1e-5 && plp(0,2)<1e-5);
// Point-point-line
Eigen::MatrixXd ppl = vision_utils::skew(p1c2)*xTi*l3;
ASSERT_TRUE(ppl(0,0)<1e-5 && ppl(1,0)<1e-5 && ppl(2,0)<1e-5);
// Point-point-point
Eigen::MatrixXd ppp = vision_utils::skew(p1c2)*xTi*vision_utils::skew(p1c3);
for (int ii=0;ii<ppp.rows(); ++ii)
ASSERT_TRUE(ppp(ii,0)<1e-5 && ppp(ii,1)<1e-5 && ppp(ii,2)<1e-5);
}
int main(int argc, char **argv)
......
......@@ -93,6 +93,7 @@ public:
;
std::string type;
std::string name;
EIGEN_MAKE_ALIGNED_OPERATOR_NEW; // to guarantee alignment (see http://eigen.tuxfamily.org/dox-devel/group__TopicStructHavingEigenMembers.html)
};
/**
......@@ -111,6 +112,8 @@ public:
*/
double getTime(void);
EIGEN_MAKE_ALIGNED_OPERATOR_NEW; // to guarantee alignment (see http://eigen.tuxfamily.org/dox-devel/group__TopicStructHavingEigenMembers.html)
protected:
double comp_time_; // Detection time
......
0% Loading or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment