| Line 6... |
Line 6... |
| 6 |
|
6 |
|
| 7 |
#include <stdio.h>
|
7 |
#include <stdio.h>
|
| 8 |
|
8 |
|
| 9 |
namespace cvtools{
|
9 |
namespace cvtools{
|
| 10 |
|
10 |
|
| - |
|
11 |
// Convert a 3xN matrix to a vector of Point3fs.
|
| - |
|
12 |
void matToPoints3f(const cv::Mat &mat, std::vector<cv::Point3f> &points){
|
| - |
|
13 |
|
| - |
|
14 |
unsigned int nPoints = mat.cols;
|
| - |
|
15 |
points.resize(nPoints);
|
| - |
|
16 |
|
| - |
|
17 |
for(unsigned int i=0; i<nPoints; i++)
|
| - |
|
18 |
points[i] = cv::Point3f(mat.at<float>(0, i), mat.at<float>(1, i), mat.at<float>(2, i));
|
| - |
|
19 |
}
|
| - |
|
20 |
|
| - |
|
21 |
// Convert a (Dim+1)xN matrix of homogenous points to a DimxN matrix of points in non-homogenous coordinates.
|
| - |
|
22 |
void convertMatFromHomogeneous(cv::Mat &src, cv::Mat &dst){
|
| - |
|
23 |
unsigned int N = src.cols;
|
| - |
|
24 |
unsigned int Dim = src.rows-1;
|
| - |
|
25 |
dst.create(Dim, N, src.type());
|
| - |
|
26 |
for(unsigned int i=0; i<N; i++){
|
| - |
|
27 |
for(unsigned int j=0; j<Dim; j++)
|
| - |
|
28 |
dst.at<float>(j,i) = src.at<float>(j,i)/src.at<float>(Dim,i);
|
| - |
|
29 |
}
|
| - |
|
30 |
|
| - |
|
31 |
}
|
| - |
|
32 |
|
| 11 |
// Function to solve the hand-eye (or eye-in-hand) calibration problem.
|
33 |
// Function to solve the hand-eye (or eye-in-hand) calibration problem.
|
| 12 |
// Finds [Omega | tau], to minimize ||[R_mark | t_mark][Omega | tau] - [Omega | tau][R | t]||^2
|
34 |
// Finds [Omega | tau], to minimize ||[R_mark | t_mark][Omega | tau] - [Omega | tau][R | t]||^2
|
| 13 |
// Algorithm according to Tsai, Lenz, A new technique for fully autonomous and efficient 3d robotics hand-eye calibration
|
35 |
// Algorithm according to Tsai, Lenz, A new technique for fully autonomous and efficient 3d robotics hand-eye calibration
|
| 14 |
// DTU, 2014, Jakob Wilm
|
36 |
// DTU, 2014, Jakob Wilm
|
| 15 |
void handEyeCalibrationTsai(const std::vector<cv::Matx33f> R, const std::vector<cv::Vec3f> t, const std::vector<cv::Matx33f> R_mark, const std::vector<cv::Vec3f> t_mark, cv::Matx33f &Omega, cv::Vec3f &tau){
|
37 |
void handEyeCalibrationTsai(const std::vector<cv::Matx33f> R, const std::vector<cv::Vec3f> t, const std::vector<cv::Matx33f> R_mark, const std::vector<cv::Vec3f> t_mark, cv::Matx33f &Omega, cv::Vec3f &tau){
|
| 16 |
// NOT DEBUGGED!
|
- |
|
| - |
|
38 |
|
| 17 |
int N = R.size();
|
39 |
int N = R.size();
|
| 18 |
assert(N == R_mark.size());
|
40 |
assert(N == R_mark.size());
|
| 19 |
assert(N == t.size());
|
41 |
assert(N == t.size());
|
| 20 |
assert(N == t_mark.size());
|
42 |
assert(N == t_mark.size());
|
| 21 |
|
43 |
|