Skip to content
GitLab
Explore
Sign in
Primary navigation
Search or go to…
Project
vision_utils
Manage
Activity
Members
Labels
Plan
Issues
Issue boards
Milestones
Wiki
Code
Merge requests
Repository
Branches
Commits
Tags
Repository graph
Compare revisions
Snippets
Build
Pipelines
Jobs
Pipeline schedules
Artifacts
Deploy
Releases
Model registry
Operate
Environments
Monitor
Incidents
Service Desk
Analyze
Value stream analytics
Contributor analytics
CI/CD analytics
Repository analytics
Model experiments
Help
Help
Support
GitLab documentation
Compare GitLab plans
Community forum
Contribute to GitLab
Provide feedback
Keyboard shortcuts
?
Snippets
Groups
Projects
Show more breadcrumbs
labrobotica
algorithms
vision_utils
Commits
238d0b8c
Commit
238d0b8c
authored
7 years ago
by
Angel Santamaria-Navarro
Browse files
Options
Downloads
Patches
Plain Diff
WIP trifocal tensor trilinearities. only the first one missing
parent
98696ff2
No related branches found
No related tags found
No related merge requests found
Changes
3
Hide whitespace changes
Inline
Side-by-side
Showing
3 changed files
src/common_class/trifocaltensor.cpp
+6
-6
6 additions, 6 deletions
src/common_class/trifocaltensor.cpp
src/test/gtest_trifocaltensor.cpp
+75
-78
75 additions, 78 deletions
src/test/gtest_trifocaltensor.cpp
src/vision_utils.h
+3
-0
3 additions, 0 deletions
src/vision_utils.h
with
84 additions
and
84 deletions
src/common_class/trifocaltensor.cpp
+
6
−
6
View file @
238d0b8c
...
...
@@ -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
=
P
3
.
block
(
0
,
3
,
3
,
1
);
Eigen
::
Vector3d
wtc2
=
P
3
.
block
(
0
,
3
,
3
,
1
);
Eigen
::
Vector3d
wtc1
=
P
1
.
block
(
0
,
3
,
3
,
1
);
Eigen
::
Vector3d
wtc2
=
P
2
.
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)
...
...
This diff is collapsed.
Click to expand it.
src/test/gtest_trifocaltensor.cpp
+
75
−
78
View file @
238d0b8c
...
...
@@ -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.104
5
,
0.0
,
1.0
;
p1c2
<<
0.104
464721937194
,
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
)
...
...
This diff is collapsed.
Click to expand it.
src/vision_utils.h
+
3
−
0
View file @
238d0b8c
...
...
@@ -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
...
...
This diff is collapsed.
Click to expand it.
Preview
0%
Loading
Try again
or
attach a new file
.
Cancel
You are about to add
0
people
to the discussion. Proceed with caution.
Finish editing this message first!
Save comment
Cancel
Please
register
or
sign in
to comment