cv::Mat toMat(Eigen::Matrix3Xd emat){
double* p = new double[emat.rows()*emat.cols()];
int w = emat.cols();
for (int i=0;i<emat.rows();i++)
for (int j=0;j<w;j++)
p[i*w+j] = emat(i,j);
cv::Mat result(emat.rows(),emat.cols(),CV_64F,p);
return result;
}
// The input 3D points are stored as columns.
Eigen::Affine3d Find3DAffineTransform(Eigen::Matrix3Xd in, Eigen::Matrix3Xd out) {
// Default output
Eigen::Affine3d A;
A.linear() = Eigen::Matrix3d::Identity(3, 3);
A.translation() = Eigen::Vector3d::Zero();
if (in.cols() != out.cols())
throw "Find3DAffineTransform(): input data mis-match";
// First find the scale, by finding the ratio of sums of some distances,
// then bring the datasets to the same scale.
double dist_in = 0, dist_out = 0;
for (int col = 0; col < in.cols()-1; col++) {
dist_in += (in.col(col+1) - in.col(col)).norm();
dist_out += (out.col(col+1) - out.col(col)).norm();
}
if (dist_in <= 0 || dist_out <= 0)
return A;
double scale = dist_out/dist_in;
out /= scale;
// Find the centroids then shift to the origin
Eigen::Vector3d in_ctr = Eigen::Vector3d::Zero();
Eigen::Vector3d out_ctr = Eigen::Vector3d::Zero();
for (int col = 0; col < in.cols(); col++) {
in_ctr += in.col(col);
out_ctr += out.col(col);
}
in_ctr /= in.cols();
out_ctr /= out.cols();
for (int col = 0; col < in.cols(); col++) {
in.col(col) -= in_ctr;
out.col(col) -= out_ctr;
}
// SVD
Eigen::MatrixXd Cov = in * out.transpose();
Eigen::JacobiSVD<Eigen::MatrixXd> svd(Cov, Eigen::ComputeThinU | Eigen::ComputeThinV);
// Find the rotation
double d = (svd.matrixV() * svd.matrixU().transpose()).determinant();
if (d > 0)
d = 1.0;
else
d = -1.0;
Eigen::Matrix3d I = Eigen::Matrix3d::Identity(3, 3);
I(2, 2) = d;
Eigen::Matrix3d R = svd.matrixV() * I * svd.matrixU().transpose();
// The final transform
A.linear() = scale * R;
A.translation() = scale*(out_ctr - R*in_ctr);
return A;
}
void GraspSet::calculateVoxelizedShadowVectorized(const Eigen::Matrix3Xd& points,
const Eigen::Vector3d& shadow_vec, int num_shadow_points, double voxel_grid_size, Vector3iSet& shadow_set) const
{
double t0_set = omp_get_wtime();
const int n = points.cols() * num_shadow_points;
const double voxel_grid_size_mult = 1.0 / voxel_grid_size;
const double max = 1.0 / 32767.0;
// Eigen::Vector3d w;
for(int i = 0; i < n; i++)
{
const int pt_idx = i / num_shadow_points;
// const Eigen::Vector3d w = (points.col(pt_idx) + ((double) fastrand() * max) * shadow_vec) * voxel_grid_size_mult;
shadow_set.insert(((points.col(pt_idx) + ((double) fastrand() * max) * shadow_vec) * voxel_grid_size_mult).cast<int>());
}
if (MEASURE_TIME)
printf("Shadow (1 camera) calculation. Runtime: %.3f, #points: %d, num_shadow_points: %d, #shadow: %d, max #shadow: %d\n",
omp_get_wtime() - t0_set, (int) points.cols(), num_shadow_points, (int) shadow_set.size(), n);
// std::cout << "Calculated shadow for 1 camera. Runtime: " << omp_get_wtime() - t0_set << ", #points: " << n << "\n";
}
void PinholeCamera<DISTORTION_T>::projectBatch(
const Eigen::Matrix3Xd & points, Eigen::Matrix2Xd * imagePoints,
std::vector<CameraBase::ProjectionStatus> * stati) const
{
const int numPoints = points.cols();
for (int i = 0; i < numPoints; ++i) {
Eigen::Vector3d point = points.col(i);
Eigen::Vector2d imagePoint;
CameraBase::ProjectionStatus status = project(point, &imagePoint);
imagePoints->col(i) = imagePoint;
if(stati)
stati->push_back(status);
}
}
TEST(OdometryCalibration, Jacobian) {
Odometry odometry;
odometry.ux = 2.0;
odometry.uy = 3.0;
odometry.utheta = M_PI;
const Eigen::Matrix3Xd jacobian = OdometryCalibration::jacobian(odometry);
ASSERT_EQ(3, (int) jacobian.rows());
ASSERT_EQ(9, (int) jacobian.cols());
ASSERT_DOUBLE_EQ(-2.0, (double) jacobian(0, 0));
ASSERT_DOUBLE_EQ(-3.0, (double) jacobian(0, 1));
ASSERT_DOUBLE_EQ(-M_PI, (double) jacobian(0, 2));
ASSERT_DOUBLE_EQ(0.0, (double) jacobian(0, 3));
ASSERT_DOUBLE_EQ(-M_PI, (double) jacobian(2, 8));
ASSERT_DOUBLE_EQ(-15-3*M_PI, jacobian.sum());
}
