Skip to content

Commit

Permalink
Fixing TransformationEstimationSVDScale
Browse files Browse the repository at this point in the history
  • Loading branch information
sdmiller committed Apr 15, 2013
1 parent 977a3e7 commit 1d46bb4
Show file tree
Hide file tree
Showing 4 changed files with 57 additions and 35 deletions.
45 changes: 24 additions & 21 deletions registration/include/pcl/registration/impl/transformation_estimation_svd.hpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -139,27 +139,30 @@ pcl::registration::TransformationEstimationSVD<PointSource, PointTarget, Scalar>
++target_it;
}

// Call Umeyama directly from Eigen (PCL patched version until Eigen is released)
transformation_matrix = pcl::umeyama (cloud_src, cloud_tgt, false);

#if 0
source_it.reset (); target_it.reset ();
// <cloud_src,cloud_src> is the source dataset
transformation_matrix.setIdentity ();

Eigen::Matrix<Scalar, 4, 1> centroid_src, centroid_tgt;
// Estimate the centroids of source, target
compute3DCentroid (source_it, centroid_src);
compute3DCentroid (target_it, centroid_tgt);
source_it.reset (); target_it.reset ();

// Subtract the centroids from source, target
Eigen::Matrix<Scalar, Eigen::Dynamic, Eigen::Dynamic> cloud_src_demean, cloud_tgt_demean;
demeanPointCloud (source_it, centroid_src, cloud_src_demean);
demeanPointCloud (target_it, centroid_tgt, cloud_tgt_demean);

getTransformationFromCorrelation (cloud_src_demean, centroid_src, cloud_tgt_demean, centroid_tgt, transformation_matrix);
#endif
if (use_umeyama_)
{
// Call Umeyama directly from Eigen (PCL patched version until Eigen is released)
transformation_matrix = pcl::umeyama (cloud_src, cloud_tgt, false);
}
else
{
source_it.reset (); target_it.reset ();
// <cloud_src,cloud_src> is the source dataset
transformation_matrix.setIdentity ();

Eigen::Matrix<Scalar, 4, 1> centroid_src, centroid_tgt;
// Estimate the centroids of source, target
compute3DCentroid (source_it, centroid_src);
compute3DCentroid (target_it, centroid_tgt);
source_it.reset (); target_it.reset ();

// Subtract the centroids from source, target
Eigen::Matrix<Scalar, Eigen::Dynamic, Eigen::Dynamic> cloud_src_demean, cloud_tgt_demean;
demeanPointCloud (source_it, centroid_src, cloud_src_demean);
demeanPointCloud (target_it, centroid_tgt, cloud_tgt_demean);

getTransformationFromCorrelation (cloud_src_demean, centroid_src, cloud_tgt_demean, centroid_tgt, transformation_matrix);
}
}

///////////////////////////////////////////////////////////////////////////////////////////
Expand Down
23 changes: 15 additions & 8 deletions registration/include/pcl/registration/impl/transformation_estimation_svd_scale.hpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -42,16 +42,16 @@
//////////////////////////////////////////////////////////////////////////////////////////////
template <typename PointSource, typename PointTarget, typename Scalar> void
pcl::registration::TransformationEstimationSVDScale<PointSource, PointTarget, Scalar>::getTransformationFromCorrelation (
const Eigen::MatrixXf &cloud_src_demean,
const Eigen::Vector4f &centroid_src,
const Eigen::MatrixXf &cloud_tgt_demean,
const Eigen::Vector4f &centroid_tgt,
const Eigen::Matrix<Scalar, Eigen::Dynamic, Eigen::Dynamic> &cloud_src_demean,
const Eigen::Matrix<Scalar, 4, 1> &centroid_src,
const Eigen::Matrix<Scalar, Eigen::Dynamic, Eigen::Dynamic> &cloud_tgt_demean,
const Eigen::Matrix<Scalar, 4, 1> &centroid_tgt,
Matrix4 &transformation_matrix) const
{
transformation_matrix.setIdentity ();

// Assemble the correlation matrix H = source * target'
Eigen::Matrix<Scalar, 3, 3> H = (cloud_src_demean.cast<Scalar> () * cloud_tgt_demean.cast<Scalar> ().transpose ()).topLeftCorner (3, 3);
Eigen::Matrix<Scalar, 3, 3> H = (cloud_src_demean * cloud_tgt_demean.transpose ()).topLeftCorner (3, 3);

// Compute the Singular Value Decomposition
Eigen::JacobiSVD<Eigen::Matrix<Scalar, 3, 3> > svd (H, Eigen::ComputeFullU | Eigen::ComputeFullV);
Expand All @@ -68,7 +68,14 @@ pcl::registration::TransformationEstimationSVDScale<PointSource, PointTarget, Sc
Eigen::Matrix<Scalar, 3, 3> R = v * u.transpose ();

// rotated cloud
Eigen::Matrix<Scalar, Eigen::Dynamic, Eigen::Dynamic> src_ = R * cloud_src_demean.cast<Scalar> ();
Eigen::Matrix<Scalar, 4, 4> R4;
R4.block (0, 0, 3, 3) = R;
R4 (0, 3) = 0;
R4 (1, 3) = 0;
R4 (2, 3) = 0;
R4 (3, 3) = 1;

Eigen::Matrix<Scalar, Eigen::Dynamic, Eigen::Dynamic> src_ = R4 * cloud_src_demean;

float scale1, scale2;
double sum_ss = 0.0f, sum_tt = 0.0f, sum_tt_ = 0.0f;
Expand All @@ -91,8 +98,8 @@ pcl::registration::TransformationEstimationSVDScale<PointSource, PointTarget, Sc
scale2 = sum_tt_ / sum_ss;
float scale = scale2;
transformation_matrix.topLeftCorner (3, 3) = scale * R;
const Eigen::Matrix<Scalar, 3, 1> Rc (R * centroid_src.cast<Scalar> ().head (3));
transformation_matrix.block (0, 3, 3, 1) = centroid_tgt.cast<Scalar> (). head (3) - Rc;
const Eigen::Matrix<Scalar, 3, 1> Rc (R * centroid_src.head (3));
transformation_matrix.block (0, 3, 3, 1) = centroid_tgt. head (3) - Rc;
}

//#define PCL_INSTANTIATE_TransformationEstimationSVD(T,U) template class PCL_EXPORTS pcl::registration::TransformationEstimationSVD<T,U>;
Expand Down
11 changes: 9 additions & 2 deletions registration/include/pcl/registration/transformation_estimation_svd.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -63,7 +63,12 @@ namespace pcl

typedef typename TransformationEstimation<PointSource, PointTarget, Scalar>::Matrix4 Matrix4;

TransformationEstimationSVD () {};
/** \brief Constructor
* \param[in] use_umeyama Toggles whether or not to use 3rd party software*/
TransformationEstimationSVD (bool use_umeyama=true):
use_umeyama_ (use_umeyama)
{}

virtual ~TransformationEstimationSVD () {};

/** \brief Estimate a rigid rotation transformation between a source and a target point cloud using SVD.
Expand Down Expand Up @@ -137,13 +142,15 @@ namespace pcl
* \param[in] centroid_tgt the input target cloud, in Eigen format
* \param[out] transformation_matrix the resultant 4x4 rigid transformation matrix
*/
void
virtual void
getTransformationFromCorrelation (
const Eigen::Matrix<Scalar, Eigen::Dynamic, Eigen::Dynamic> &cloud_src_demean,
const Eigen::Matrix<Scalar, 4, 1> &centroid_src,
const Eigen::Matrix<Scalar, Eigen::Dynamic, Eigen::Dynamic> &cloud_tgt_demean,
const Eigen::Matrix<Scalar, 4, 1> &centroid_tgt,
Matrix4 &transformation_matrix) const;

bool use_umeyama_;
};

}
Expand Down
13 changes: 9 additions & 4 deletions registration/include/pcl/registration/transformation_estimation_svd_scale.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -63,6 +63,11 @@ namespace pcl

typedef typename TransformationEstimationSVD<PointSource, PointTarget, Scalar>::Matrix4 Matrix4;

/** \brief Inherits from TransformationEstimationSVD, but forces it to not use the Umeyama method */
TransformationEstimationSVDScale ():
TransformationEstimationSVD<PointSource, PointTarget, Scalar> (false)
{}

protected:
/** \brief Obtain a 4x4 rigid transformation matrix from a correlation matrix H = src * tgt'
* \param[in] cloud_src_demean the input source cloud, demeaned, in Eigen format
Expand All @@ -72,10 +77,10 @@ namespace pcl
* \param[out] transformation_matrix the resultant 4x4 rigid transformation matrix
*/
void
getTransformationFromCorrelation (const Eigen::MatrixXf &cloud_src_demean,
const Eigen::Vector4f &centroid_src,
const Eigen::MatrixXf &cloud_tgt_demean,
const Eigen::Vector4f &centroid_tgt,
getTransformationFromCorrelation (const Eigen::Matrix<Scalar, Eigen::Dynamic, Eigen::Dynamic> &cloud_src_demean,
const Eigen::Matrix<Scalar, 4, 1> &centroid_src,
const Eigen::Matrix<Scalar, Eigen::Dynamic, Eigen::Dynamic> &cloud_tgt_demean,
const Eigen::Matrix<Scalar, 4, 1> &centroid_tgt,
Matrix4 &transformation_matrix) const;
};

Expand Down

0 comments on commit 1d46bb4

Please sign in to comment.