Exemplo n.º 1
0
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;
}
Exemplo n.º 2
0
// 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;
}
Exemplo n.º 3
0
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";
}
Exemplo n.º 4
0
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());
}