template <typename PointSource, typename PointTarget> void
pcl::registration::TransformationEstimationSVD<PointSource, PointTarget>::getTransformationFromCorrelation (
const Eigen::MatrixXf &cloud_src_demean,
const Eigen::Vector4f ¢roid_src,
const Eigen::MatrixXf &cloud_tgt_demean,
const Eigen::Vector4f ¢roid_tgt,
Eigen::Matrix4f &transformation_matrix)
{
transformation_matrix.setIdentity ();
// Assemble the correlation matrix H = source * target'
Eigen::Matrix3f H = (cloud_src_demean * cloud_tgt_demean.transpose ()).topLeftCorner<3, 3>();
// Compute the Singular Value Decomposition
Eigen::JacobiSVD<Eigen::Matrix3f> svd (H, Eigen::ComputeFullU | Eigen::ComputeFullV);
Eigen::Matrix3f u = svd.matrixU ();
Eigen::Matrix3f v = svd.matrixV ();
// Compute R = V * U'
if (u.determinant () * v.determinant () < 0)
{
for (int x = 0; x < 3; ++x)
v (x, 2) *= -1;
}
Eigen::Matrix3f R = v * u.transpose ();
// Return the correct transformation
transformation_matrix.topLeftCorner<3, 3> () = R;
Eigen::Vector3f Rc = R * centroid_src.head<3> ();
transformation_matrix.block <3, 1> (0, 3) = centroid_tgt.head<3> () - Rc;
}
/** Extract rotation (R) and translation (t) from a given essential matrix and triangulate feature matches.
* @param Eigen::Matrix3f essential matrix
* @param std::vector<Match> vector with matches
* @param Eigen::Matrix3f output rotation matrix
* @param Eigen::Vector3f output translation vector
* @param Eigen::Matrix<float, 4, Eigen::Dynamic> output matrix with computed 3D points
* @return void */
void MonoOdometer5::E2Rt(Eigen::Matrix3f E, std::vector<Match> matches, Eigen::Matrix3f &R, Eigen::Vector3f &t, Eigen::Matrix<float, 4, Eigen::Dynamic> &points3D)
{
Eigen::Matrix3f W, Z;
W << 0, -1, 0, 1, 0, 0, 0, 0, 1;
Z << 0, 1, 0, -1, 0, 0, 0, 0, 0;
// singular values decomposition of E
Eigen::JacobiSVD<Eigen::Matrix3f> svdE(E, Eigen::ComputeFullU | Eigen::ComputeFullV);
// get skew symmetric matrix of translation vector
Eigen::Matrix3f S = svdE.matrixU() * Z * (svdE.matrixU()).transpose();
// get possible rotation matrices
Eigen::Matrix3f Ra = svdE.matrixU() * W * (svdE.matrixV()).transpose();
Eigen::Matrix3f Rb = svdE.matrixU() * W.transpose() * (svdE.matrixV()).transpose();
// get translation vetor from skew symmetric matrix
t << S(2, 1), S(0, 2), S(1, 0);
// correct signal of both rotation matrices
if(Ra.determinant() < 0)
{
Ra = -Ra;
}
if(Rb.determinant() < 0)
{
Rb = -Rb;
}
// 4 possible combinations of R,t
std::vector<Eigen::Matrix3f> solutionsR;
std::vector<Eigen::Vector3f> solutionst;
solutionsR.push_back(Ra); solutionst.push_back(t);
solutionsR.push_back(Ra); solutionst.push_back(-t);
solutionsR.push_back(Rb); solutionst.push_back(t);
solutionsR.push_back(Rb); solutionst.push_back(-t);
// test Chirality constraint for all 4 solutions
Eigen::Matrix<float, 4, Eigen::Dynamic> points3DCurr;
int maxInliers = 0;
for(int i=0; i<4; i++)
{
int nInliers = triangulate(matches, solutionsR[i], solutionst[i], points3DCurr);
// solution with the most inliers wins
if(nInliers > maxInliers)
{
maxInliers = nInliers;
points3D = points3DCurr;
R = solutionsR[i];
t = solutionst[i];
}
}
}
/**
* estimateRigidTransformationSVD
*/
void RigidTransformationRANSAC::estimateRigidTransformationSVD(
const std::vector<Eigen::Vector3f > &srcPts,
const std::vector<int> &srcIndices,
const std::vector<Eigen::Vector3f > &tgtPts,
const std::vector<int> &tgtIndices,
Eigen::Matrix4f &transform)
{
Eigen::Vector3f srcCentroid, tgtCentroid;
// compute the centroids of source, target
ComputeCentroid (srcPts, srcIndices, srcCentroid);
ComputeCentroid (tgtPts, tgtIndices, tgtCentroid);
// Subtract the centroids from source, target
Eigen::MatrixXf srcPtsDemean;
DemeanPoints(srcPts, srcIndices, srcCentroid, srcPtsDemean);
Eigen::MatrixXf tgtPtsDemean;
DemeanPoints(tgtPts, tgtIndices, tgtCentroid, tgtPtsDemean);
// Assemble the correlation matrix H = source * target'
Eigen::Matrix3f H = (srcPtsDemean * tgtPtsDemean.transpose ()).topLeftCorner<3, 3>();
// Singular Value Decomposition
Eigen::JacobiSVD<Eigen::Matrix3f> svd (H, Eigen::ComputeFullU | Eigen::ComputeFullV);
Eigen::Matrix3f u = svd.matrixU ();
Eigen::Matrix3f v = svd.matrixV ();
// Compute R = V * U'
if (u.determinant () * v.determinant () < 0)
{
for (int x = 0; x < 3; ++x)
v (x, 2) *= -1;
}
// Return the transformation
Eigen::Matrix3f R = v * u.transpose ();
Eigen::Vector3f t = tgtCentroid - R * srcCentroid;
// set rotation
transform.block(0,0,3,3) = R;
// set translation
transform.block(0,3,3,1) = t;
transform.block(3, 0, 1, 3).setZero();
transform(3,3) = 1.;
}
template <typename PointT> void
pcl::SampleConsensusModelRegistration<PointT>::estimateRigidTransformationSVD (
const pcl::PointCloud<PointT> &cloud_src,
const std::vector<int> &indices_src,
const pcl::PointCloud<PointT> &cloud_tgt,
const std::vector<int> &indices_tgt,
Eigen::VectorXf &transform)
{
transform.resize (16);
Eigen::Vector4f centroid_src, centroid_tgt;
// Estimate the centroids of source, target
compute3DCentroid (cloud_src, indices_src, centroid_src);
compute3DCentroid (cloud_tgt, indices_tgt, centroid_tgt);
// Subtract the centroids from source, target
Eigen::MatrixXf cloud_src_demean;
demeanPointCloud (cloud_src, indices_src, centroid_src, cloud_src_demean);
Eigen::MatrixXf cloud_tgt_demean;
demeanPointCloud (cloud_tgt, indices_tgt, centroid_tgt, cloud_tgt_demean);
// Assemble the correlation matrix H = source * target'
Eigen::Matrix3f H = (cloud_src_demean * cloud_tgt_demean.transpose ()).topLeftCorner<3, 3>();
// Compute the Singular Value Decomposition
Eigen::JacobiSVD<Eigen::Matrix3f> svd (H, Eigen::ComputeFullU | Eigen::ComputeFullV);
Eigen::Matrix3f u = svd.matrixU ();
Eigen::Matrix3f v = svd.matrixV ();
// Compute R = V * U'
if (u.determinant () * v.determinant () < 0)
{
for (int x = 0; x < 3; ++x)
v (x, 2) *= -1;
}
Eigen::Matrix3f R = v * u.transpose ();
// Return the correct transformation
transform.segment<3> (0) = R.row (0); transform[12] = 0;
transform.segment<3> (4) = R.row (1); transform[13] = 0;
transform.segment<3> (8) = R.row (2); transform[14] = 0;
Eigen::Vector3f t = centroid_tgt.head<3> () - R * centroid_src.head<3> ();
transform[3] = t[0]; transform[7] = t[1]; transform[11] = t[2]; transform[15] = 1.0;
}
void Element::CalculateStiffnessMatrix(const Eigen::Matrix3f& D, std::vector<Eigen::Triplet<float> >& triplets)
{
Eigen::Vector3f x, y;
x << nodesX[nodesIds[0]], nodesX[nodesIds[1]], nodesX[nodesIds[2]];
y << nodesY[nodesIds[0]], nodesY[nodesIds[1]], nodesY[nodesIds[2]];
Eigen::Matrix3f C;
C << Eigen::Vector3f(1.0f, 1.0f, 1.0f), x, y;
Eigen::Matrix3f IC = C.inverse();
for (int i = 0; i < 3; i++)
{
B(0, 2 * i + 0) = IC(1, i);
B(0, 2 * i + 1) = 0.0f;
B(1, 2 * i + 0) = 0.0f;
B(1, 2 * i + 1) = IC(2, i);
B(2, 2 * i + 0) = IC(2, i);
B(2, 2 * i + 1) = IC(1, i);
}
Eigen::Matrix<float, 6, 6> K = B.transpose() * D * B * C.determinant() / 2.0f;
for (int i = 0; i < 3; i++)
{
for (int j = 0; j < 3; j++)
{
Eigen::Triplet<float> trplt11(2 * nodesIds[i] + 0, 2 * nodesIds[j] + 0, K(2 * i + 0, 2 * j + 0));
Eigen::Triplet<float> trplt12(2 * nodesIds[i] + 0, 2 * nodesIds[j] + 1, K(2 * i + 0, 2 * j + 1));
Eigen::Triplet<float> trplt21(2 * nodesIds[i] + 1, 2 * nodesIds[j] + 0, K(2 * i + 1, 2 * j + 0));
Eigen::Triplet<float> trplt22(2 * nodesIds[i] + 1, 2 * nodesIds[j] + 1, K(2 * i + 1, 2 * j + 1));
triplets.push_back(trplt11);
triplets.push_back(trplt12);
triplets.push_back(trplt21);
triplets.push_back(trplt22);
}
}
}
void onGMM(const gaussian_mixture_model::GaussianMixture & mix)
{
visualization_msgs::MarkerArray msg;
ROS_INFO("gmm_rviz_converter: Received message.");
uint i;
for (i = 0; i < mix.gaussians.size(); i++)
{
visualization_msgs::Marker marker;
marker.header.frame_id = m_frame_id;
marker.header.stamp = ros::Time::now();
marker.ns = m_rviz_namespace;
marker.id = i;
marker.type = visualization_msgs::Marker::SPHERE;
marker.action = visualization_msgs::Marker::ADD;
marker.lifetime = ros::Duration();
Eigen::Vector3f coords;
for (uint ir = 0; ir < NUM_OUTPUT_COORDINATES; ir++)
coords[ir] = m_conversion_mask[ir]->GetMean(m_conversion_mask,mix.gaussians[i]);
marker.pose.position.x = coords.x();
marker.pose.position.y = coords.y();
marker.pose.position.z = coords.z();
Eigen::Matrix3f covmat;
for (uint ir = 0; ir < NUM_OUTPUT_COORDINATES; ir++)
for (uint ic = 0; ic < NUM_OUTPUT_COORDINATES; ic++)
covmat(ir,ic) = m_conversion_mask[ir]->GetCov(m_conversion_mask,mix.gaussians[i],ic);
Eigen::EigenSolver<Eigen::Matrix3f> evsolver(covmat);
Eigen::Matrix3f eigenvectors = evsolver.eigenvectors().real();
if (eigenvectors.determinant() < 0.0)
eigenvectors.col(0) = - eigenvectors.col(0);
Eigen::Matrix3f rotation = eigenvectors;
Eigen::Quaternionf quat = Eigen::Quaternionf(Eigen::AngleAxisf(rotation));
marker.pose.orientation.x = quat.x();
marker.pose.orientation.y = quat.y();
marker.pose.orientation.z = quat.z();
marker.pose.orientation.w = quat.w();
Eigen::Vector3f eigenvalues = evsolver.eigenvalues().real();
Eigen::Vector3f scale = Eigen::Vector3f(eigenvalues.array().abs().sqrt());
if (m_normalize)
scale.normalize();
marker.scale.x = mix.weights[i] * scale.x() * m_scale;
marker.scale.y = mix.weights[i] * scale.y() * m_scale;
marker.scale.z = mix.weights[i] * scale.z() * m_scale;
marker.color.a = 1.0;
rainbow(float(i) / float(mix.gaussians.size()),marker.color.r,marker.color.g,marker.color.b);
msg.markers.push_back(marker);
}
// this a waste of resources, but we need to delete old markers in some way
// someone should add a "clear all" command to rviz
// (using expiration time is not an option, here)
for ( ; i < m_max_markers; i++)
{
visualization_msgs::Marker marker;
marker.id = i;
marker.action = visualization_msgs::Marker::DELETE;
marker.lifetime = ros::Duration();
marker.header.frame_id = m_frame_id;
marker.header.stamp = ros::Time::now();
marker.ns = m_rviz_namespace;
msg.markers.push_back(marker);
}
m_pub.publish(msg);
ROS_INFO("gmm_rviz_converter: Sent message.");
}
void cloud_cb (const sensor_msgs::PointCloud2ConstPtr& input)
{
// Create a container for the data.
pcl::PointCloud<pcl::PointXYZRGBNormal> cloud;
pcl::PointCloud<pcl::PointXYZRGBNormal> cloud_fast;
pcl::fromROSMsg(*input, cloud);
for (size_t i=0; i < cloud.points.size(); i++) {
pcl::PointXYZRGBNormal point_temp = cloud.points[i];
if (sqrt(point_temp.normal_x*point_temp.normal_x+point_temp.normal_y*point_temp.normal_y+point_temp.normal_z*point_temp.normal_z) > 0.02) {
cloud_fast.points.push_back(cloud.points[i]);
}
}
cloud_fast.width=cloud_fast.points.size();
cloud_fast.height=1;
ROS_INFO("done extract fast size %d", cloud_fast.points.size());
unsigned int max_inliers = 0;
Eigen::Matrix3f R_best;
Eigen::Vector3f t_best;
pcl::PointCloud<pcl::PointXYZRGBNormal> point_best;
point_best.points.resize(3); point_best.width = 3; point_best.height = 1;
bool apply_sac;
int mirror_num = 0;
if (cloud_fast.points.size() > 50) {
omp_lock_t writelock;
omp_init_lock(&writelock);
#ifdef _OPENMP
#pragma omp parallel for
#endif
for (size_t i=0; i < 2000; i++) {
unsigned int rand_nums[3];
while (true) {
rand_nums[0] = rand() % cloud_fast.width;
rand_nums[1] = rand() % cloud_fast.width;
rand_nums[2] = rand() % cloud_fast.width;
if (rand_nums[0] != rand_nums[1] && rand_nums[1] != rand_nums[2] && rand_nums[2] != rand_nums[0])
break;
}
Eigen::Vector3f v_before[3];
Eigen::Vector3f v_after[3];
for (size_t j=0; j < 3; j++) {
pcl::PointXYZRGBNormal point_temp = cloud_fast.points[rand_nums[j]];
v_before[j] = Eigen::Vector3f(point_temp.x, point_temp.y, point_temp.z);
v_after[j] = v_before[j] + Eigen::Vector3f(point_temp.normal_x, point_temp.normal_y, point_temp.normal_z);
}
Eigen::Vector3f v_before_ave = (v_before[0] + v_before[1] + v_before[2]) / 3.0;
Eigen::Vector3f v_after_ave = (v_after[0] + v_after[1] + v_after[2]) / 3.0;
Eigen::Vector3f v_before_relative[3];
Eigen::Vector3f v_after_relative[3];
Eigen::MatrixXf m_before_ave = Eigen::MatrixXf::Zero(3, 3);
Eigen::MatrixXf m_after_ave = Eigen::MatrixXf::Zero(3, 3);
for (size_t j=0; j < 3; j++) {
v_before_relative[j] = v_before[j] - v_before_ave;
v_after_relative[j] = v_after[j] - v_after_ave;
m_before_ave.col(j) = v_before_relative[j];
m_after_ave.col(j) = v_after_relative[j];
}
Eigen::Matrix3f Q = m_after_ave * m_before_ave.transpose();
JacobiSVD<Eigen::Matrix3f> svd(Q, ComputeFullU | ComputeFullV);
Eigen::Matrix3f R = svd.matrixV() * svd.matrixU().transpose();
if (R.determinant() < 0.0) {mirror_num++; continue;}
Eigen::Vector3f t = v_after_ave - R * v_before_ave;
unsigned int inliers = 0;
for (size_t j=0; j < cloud_fast.points.size(); j++) {
pcl::PointXYZRGBNormal point_temp = cloud_fast.points[j];
Eigen::Vector3f v_before_temp = Eigen::Vector3f(point_temp.x, point_temp.y, point_temp.z);
Eigen::Vector3f v_after_temp = v_before_temp + Eigen::Vector3f(point_temp.normal_x, point_temp.normal_y, point_temp.normal_z);
Eigen::Vector3f v_after_temp_estimated = R * v_before_temp + t;
float error = (v_after_temp_estimated - v_after_temp).norm();
if (error < 0.03){
inliers ++;
}
}
omp_set_lock(&writelock);
if (max_inliers < inliers) {
max_inliers = inliers;
R_best = R;
t_best = t;
for (size_t j=0; j < 3; j++) {
point_best.points[j]=cloud_fast.points[rand_nums[j]];
}
}
omp_unset_lock(&writelock);
}
ROS_INFO("Done sac, total moving: %d, max_i: %d", cloud_fast.points.size(), max_inliers);
apply_sac = true;
ROS_INFO("determinant %f", R_best.determinant());
//ROS_INFO("mirror num %d", mirror_num);
} else{
ROS_INFO("No moving points");
apply_sac = false;
}
sensor_msgs::PointCloud2 ros_out;
pcl::toROSMsg(cloud_fast, ros_out);
ros_out.header = input->header;
pub_result_cloud_fast_.publish(ros_out);
if (apply_sac){
pcl::PointCloud<pcl::PointXYZRGBNormal> cloud_sac, cloud_sac2, cloud_sac3, cloud_sac4;
for (size_t j=0; j < cloud.points.size(); j++) {
pcl::PointXYZRGBNormal point_temp = cloud.points[j];
Eigen::Vector3f v_before_temp = Eigen::Vector3f(point_temp.x, point_temp.y, point_temp.z);
Eigen::Vector3f v_after_temp = v_before_temp + Eigen::Vector3f(point_temp.normal_x, point_temp.normal_y, point_temp.normal_z);
Eigen::Vector3f v_after_temp_estimated = R_best * v_before_temp + t_best;
float error = (v_after_temp_estimated - v_after_temp).norm();
if (error < 0.03){
cloud_sac.points.push_back(cloud.points[j]);
pcl::PointXYZRGBNormal point_temp2;
point_temp2.x = v_after_temp_estimated[0];
point_temp2.y = v_after_temp_estimated[1];
point_temp2.z = v_after_temp_estimated[2];
cloud_sac2.points.push_back(point_temp2);
v_after_temp_estimated = R_best * v_after_temp_estimated + t_best;
pcl::PointXYZRGBNormal point_temp3;
point_temp3.x = v_after_temp_estimated[0];
point_temp3.y = v_after_temp_estimated[1];
point_temp3.z = v_after_temp_estimated[2];
cloud_sac3.points.push_back(point_temp3);
v_after_temp_estimated = R_best * v_after_temp_estimated + t_best;
pcl::PointXYZRGBNormal point_temp4;
point_temp4.x = v_after_temp_estimated[0];
point_temp4.y = v_after_temp_estimated[1];
point_temp4.z = v_after_temp_estimated[2];
cloud_sac4.points.push_back(point_temp4);
// if (j == 20) {
// Eigen::Vector3f v_a_b = v_after_temp_estimated4;
// for (int k=0; k<10; k++){
// v_a_b = R_best * v_a_b + t_best;
// ROS_INFO("v_a_b, x:%f, y:%f, z:%f", v_a_b[0], v_a_b[1], v_a_b[2]);
// }
// }
}
}
cloud_sac.width=cloud_sac2.width=cloud_sac3.width=cloud_sac4.width=cloud_sac.points.size();
cloud_sac.height=cloud_sac2.height=cloud_sac3.height=cloud_sac4.height=1;
pcl::toROSMsg(cloud_sac, ros_out);
ros_out.header = input->header;
pub_result_cloud_sac_.publish(ros_out);
pcl::toROSMsg(cloud_sac2, ros_out);
ros_out.header = input->header;
pub_result_cloud_sac2_.publish(ros_out);
pcl::toROSMsg(cloud_sac3, ros_out);
ros_out.header = input->header;
pub_result_cloud_sac3_.publish(ros_out);
pcl::toROSMsg(cloud_sac4, ros_out);
ros_out.header = input->header;
pub_result_cloud_sac4_.publish(ros_out);
pcl::toROSMsg(point_best, ros_out);
ros_out.header = input->header;
pub_result_cloud_seed_.publish(ros_out);
visualization_msgs::MarkerArray marker_array_best;
for (int i=0; i<3; i++) {
visualization_msgs::Marker marker_best;
geometry_msgs::Point point;
marker_best.header = input->header;
marker_best.type = visualization_msgs::Marker::ARROW;
point.x = point_best.points[i].x;
point.y = point_best.points[i].y;
point.z = point_best.points[i].z;
marker_best.points.push_back(point);
point.x = point_best.points[i].x + point_best.points[i].normal_x;
point.y = point_best.points[i].y + point_best.points[i].normal_y;
point.z = point_best.points[i].z + point_best.points[i].normal_z;
marker_best.points.push_back(point);
marker_best.id = i;
marker_best.ns = std::string("debug_point_array: ");
marker_best.scale.x = 0.01;
marker_best.scale.y = 0.02;
marker_best.scale.z = 0.01;
marker_best.color.r = 1.0;
marker_best.color.a = 1.0;
marker_array_best.markers.push_back(marker_best);
}
pub_result_pose_seed_.publish(marker_array_best);
}
}