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()); }