visualization_msgs::Marker SatDetector3DMonitor::getMarker(int marker_id, Eigen::Vector4f centroid, Eigen::Matrix3f covariance_matrix){
//------- Compute Principal Componets for Marker publishing
//Get the principal components and the quaternion
Eigen::Matrix3f evecs;
Eigen::Vector3f evals;
//pcl::eigen33 (covariance_matrix, evecs, evals);
Eigen::EigenSolver<Eigen::Matrix3f> es(covariance_matrix);
evecs = es.eigenvectors().real();
evals = es.eigenvalues().real();
Eigen::Matrix3f rotation;
rotation.row (0) = evecs.col (0);
rotation.row (1) = evecs.col (1);
rotation.row (2) = rotation.row (0).cross (rotation.row (1));
rotation.transposeInPlace ();
Eigen::Quaternion<float> qt (rotation);
qt.normalize ();
//Publish Marker for cluster
visualization_msgs::Marker marker;
marker.header.frame_id = base_frame_;
marker.header.stamp = ros::Time().now();
marker.ns = "Perception";
marker.action = visualization_msgs::Marker::ADD;
marker.id = marker_id;
marker.lifetime = ros::Duration(1);
//centroid position
marker.type = visualization_msgs::Marker::SPHERE;
marker.pose.position.x = centroid[0];
marker.pose.position.y = centroid[1];
marker.pose.position.z = centroid[2];
marker.pose.orientation.x = qt.x();
marker.pose.orientation.y = qt.y();
marker.pose.orientation.z = qt.z();
marker.pose.orientation.w = qt.w();
//std::cout << "e1: " << evals(0) << " e2: " << evals(1) << " e3: " << evals(2) << std::endl;
marker.scale.x = sqrt(evals(0)) * 4;
marker.scale.y = sqrt(evals(1)) * 4;
marker.scale.z = sqrt(evals(2)) * 4;
//give it some color!
marker.color.a = 0.75;
satToRGB(marker.color.r, marker.color.g, marker.color.b);
//std::cout << "marker being published" << std::endl;
return marker;
}
template <typename PointInT, typename PointOutT> void
pcl::SHOTLocalReferenceFrameEstimation<PointInT, PointOutT>::computeFeature (PointCloudOut &output)
{
//check whether used with search radius or search k-neighbors
if (this->getKSearch () != 0)
{
PCL_ERROR(
"[pcl::%s::computeFeature] Error! Search method set to k-neighborhood. Call setKSearch(0) and setRadiusSearch( radius ) to use this class.\n",
getClassName().c_str ());
return;
}
tree_->setSortedResults (true);
for (size_t i = 0; i < indices_->size (); ++i)
{
// point result
Eigen::Matrix3f rf;
PointOutT& output_rf = output[i];
//output_rf.confidence = getLocalRF ((*indices_)[i], rf);
//if (output_rf.confidence == std::numeric_limits<float>::max ())
if (getLocalRF ((*indices_)[i], rf) == std::numeric_limits<float>::max ())
{
output.is_dense = false;
}
output_rf.x_axis.getNormalVector3fMap () = rf.row (0);
output_rf.y_axis.getNormalVector3fMap () = rf.row (1);
output_rf.z_axis.getNormalVector3fMap () = rf.row (2);
}
}
template<typename PointInT, typename PointNT, typename PointOutT> void
pcl::BOARDLocalReferenceFrameEstimation<PointInT, PointNT, PointOutT>::computeFeature (PointCloudOut &output)
{
//check whether used with search radius or search k-neighbors
if (this->getKSearch () != 0)
{
PCL_ERROR(
"[pcl::%s::computeFeature] Error! Search method set to k-neighborhood. Call setKSearch(0) and setRadiusSearch( radius ) to use this class.\n",
getClassName().c_str());
return;
}
this->resetData ();
for (size_t point_idx = 0; point_idx < indices_->size (); ++point_idx)
{
Eigen::Matrix3f currentLrf;
PointOutT &rf = output[point_idx];
//rf.confidence = computePointLRF (*indices_[point_idx], currentLrf);
//if (rf.confidence == std::numeric_limits<float>::max ())
if (computePointLRF ((*indices_)[point_idx], currentLrf) == std::numeric_limits<float>::max ())
{
output.is_dense = false;
}
rf.x_axis.getNormalVector3fMap () = currentLrf.row (0);
rf.y_axis.getNormalVector3fMap () = currentLrf.row (1);
rf.z_axis.getNormalVector3fMap () = currentLrf.row (2);
}
}
void
pcl::SHOTLocalReferenceFrameEstimationOMP<PointInT, PointOutT>::computeFeatureEigen (pcl::PointCloud<Eigen::MatrixXf> &output)
{
if (threads_ < 0)
threads_ = 1;
//check whether used with search radius or search k-neighbors
if (this->getKSearch () != 0)
{
PCL_ERROR(
"[pcl::%s::computeFeatureEigen] Error! Search method set to k-neighborhood. Call setKSearch(0) and setRadiusSearch( radius ) to use this class.\n",
getClassName().c_str ());
return;
}
tree_->setSortedResults (true);
int data_size = static_cast<int> (indices_->size ());
// Set up the output channels
output.channels["shot_lrf"].name = "shot_lrf";
output.channels["shot_lrf"].offset = 0;
output.channels["shot_lrf"].size = 4;
output.channels["shot_lrf"].count = 9;
output.channels["shot_lrf"].datatype = sensor_msgs::PointField::FLOAT32;
//output.points.resize (indices_->size (), 10);
output.points.resize (data_size, 9);
#pragma omp parallel for num_threads(threads_)
for (int i = 0; i < data_size; ++i)
{
// point result
Eigen::Matrix3f rf;
//output.points (i, 9) = getLocalRF ((*indices_)[i], rf);
//if (output.points (i, 9) == std::numeric_limits<float>::max ())
if (getLocalRF ((*indices_)[i], rf) == std::numeric_limits<float>::max ())
{
output.is_dense = false;
}
output.points.block<1, 3> (i, 0) = rf.row (0);
output.points.block<1, 3> (i, 3) = rf.row (1);
output.points.block<1, 3> (i, 6) = rf.row (2);
}
}
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
pcl::SHOTLocalReferenceFrameEstimationOMP<PointInT, PointOutT>::computeFeature (PointCloudOut &output)
{
if (threads_ < 0)
threads_ = 1;
//check whether used with search radius or search k-neighbors
if (this->getKSearch () != 0)
{
PCL_ERROR(
"[pcl::%s::computeFeature] Error! Search method set to k-neighborhood. Call setKSearch(0) and setRadiusSearch( radius ) to use this class.\n",
getClassName().c_str ());
return;
}
tree_->setSortedResults (true);
int data_size = static_cast<int> (indices_->size ());
#pragma omp parallel for num_threads(threads_)
for (int i = 0; i < data_size; ++i)
{
// point result
Eigen::Matrix3f rf;
PointOutT& output_rf = output[i];
//output_rf.confidence = getLocalRF ((*indices_)[i], rf);
//if (output_rf.confidence == std::numeric_limits<float>::max ())
std::vector<int> n_indices;
std::vector<float> n_sqr_distances;
this->searchForNeighbors ((*indices_)[i], search_parameter_, n_indices, n_sqr_distances);
if (getLocalRF ((*indices_)[i], rf) == std::numeric_limits<float>::max ())
{
output.is_dense = false;
}
output_rf.x_axis.getNormalVector3fMap () = rf.row (0);
output_rf.y_axis.getNormalVector3fMap () = rf.row (1);
output_rf.z_axis.getNormalVector3fMap () = rf.row (2);
}
}
template<typename PointInT, typename PointNT> void
pcl::BOARDLocalReferenceFrameEstimation<PointInT, PointNT, Eigen::MatrixXf>::computeFeatureEigen (
pcl::PointCloud<
Eigen::MatrixXf> &output)
{
//check whether used with search radius or search k-neighbors
if (this->getKSearch () != 0)
{
PCL_ERROR(
"[pcl::%s::computeFeatureEigen] Error! Search method set to k-neighborhood. Call setKSearch(0) and setRadiusSearch( radius ) to use this class.\n",
getClassName().c_str());
return;
}
this->resetData ();
// Set up the output channels
output.channels["board"].name = "board";
output.channels["board"].offset = 0;
output.channels["board"].size = 4;
output.channels["board"].count = 9;
output.channels["board"].datatype = sensor_msgs::PointField::FLOAT32;
//output.points.resize (indices_->size (), 10);
output.points.resize (indices_->size (), 9);
for (size_t point_idx = 0; point_idx < indices_->size (); ++point_idx)
{
Eigen::Matrix3f currentLrf;
//output.points (point_idx, 9) = computePointLRF (*indices_[point_idx], currentLrf);
//if (output.points (point_idx, 9) == std::numeric_limits<float>::max ())
if (this->computePointLRF ((*indices_)[point_idx], currentLrf) == std::numeric_limits<float>::max ())
{
output.is_dense = false;
}
output.points.block<1, 3> (point_idx, 0) = currentLrf.row (0);
output.points.block<1, 3> (point_idx, 3) = currentLrf.row (1);
output.points.block<1, 3> (point_idx, 6) = currentLrf.row (2);
}
}
bool Utils::
factorViewMatrix(const Eigen::Projective3f& iMatrix,
Eigen::Matrix3f& oCalib, Eigen::Isometry3f& oPose,
bool& oIsOrthographic) {
oIsOrthographic = isOrthographic(iMatrix.matrix());
// get appropriate rows
std::vector<int> rows = {0,1,2};
if (!oIsOrthographic) rows[2] = 3;
// get A matrix (upper left 3x3) and t vector
Eigen::Matrix3f A;
Eigen::Vector3f t;
for (int i = 0; i < 3; ++i) {
for (int j = 0; j < 3; ++j) {
A(i,j) = iMatrix(rows[i],j);
}
t[i] = iMatrix(rows[i],3);
}
// determine translation vector
oPose.setIdentity();
oPose.translation() = -(A.inverse()*t);
// determine calibration matrix
Eigen::Matrix3f AAtrans = A*A.transpose();
AAtrans.col(0).swap(AAtrans.col(2));
AAtrans.row(0).swap(AAtrans.row(2));
Eigen::LLT<Eigen::Matrix3f, Eigen::Upper> llt(AAtrans);
oCalib = llt.matrixU();
oCalib.col(0).swap(oCalib.col(2));
oCalib.row(0).swap(oCalib.row(2));
oCalib.transposeInPlace();
// compute rotation matrix
oPose.linear() = (oCalib.inverse()*A).transpose();
return true;
}
int main( )
{
Eigen::VectorXf a(3);
Eigen::VectorXf b(a);
Eigen::VectorXf c(b);
a = Eigen::Vector4f(1, 2, 3, 4);
b = Eigen::Vector4f(3, 2, 1, 0);
c = a + b;
Eigen::Matrix<float,Eigen::Dynamic,Eigen::Dynamic> x(1,3);
Eigen::Matrix<float,Eigen::Dynamic,Eigen::Dynamic> y(3,1);
x = y;
x.resize(4,4);
x.resize(2,3);
y = x;
y = a;
Eigen::Vector3f d = a.block<3, 1>(0, 0);
c = a;
Eigen::Vector3f e = a.block<3, 1>(0, 0);
Eigen::ArrayXf ddd = c;
ddd = a;
Eigen::Matrix3f f;
f.row(0) = e;
Eigen::Affine3f m;
m.matrix().row(0).head<3>() = e;
return EXIT_SUCCESS;
}
osgpcl::PointCloudGeometry* osgpcl::SurfelFactoryFF<PointT, NormalT, RadiusT>::buildGeometry(
bool unique_state) {
typename pcl::PointCloud<NormalT>::ConstPtr normals = this->getInputCloud<NormalT>();
typename pcl::PointCloud<PointT>::ConstPtr xyz = this->getInputCloud<PointT>();
typename pcl::PointCloud<RadiusT>::ConstPtr rads = this->getInputCloud<RadiusT>();
if (xyz ==NULL) return NULL;
if (normals ==NULL) return NULL;
if (rads ==NULL) return NULL;
if (xyz->points.size() != normals->points.size()) return NULL;
if (rads->points.size() != normals->points.size()) return NULL;
pcl::IndicesConstPtr indices = indices_;
{
bool rebuild_indices= false;
if (indices_ == NULL) rebuild_indices=true;
else if (indices_ ->size() != xyz->points.size() ) rebuild_indices=true;
if (rebuild_indices){
pcl::IndicesPtr idxs(new std::vector<int>);
idxs->reserve(xyz->points.size());
for(int i=0; i<xyz->points.size(); i++) idxs->push_back(i);
indices= idxs;
}
}
osg::Vec3Array* pts = new osg::Vec3Array;
osg::Vec3Array* npts = new osg::Vec3Array;
int fan_size = 1+ circle_cache.rows();
pts->reserve(indices->size()*fan_size );
npts->reserve(indices->size()*fan_size);
osg::Geometry* geom = new osg::Geometry;
for(int i=0, pstart=0; i<indices->size(); i++){
const int& idx = (*indices)[i];
const PointT& pt = xyz->points[idx];
const NormalT& npt = normals->points[idx];
pts->push_back(osg::Vec3(pt.x, pt.y, pt.z));
npts->push_back(osg::Vec3(npt.normal_x, npt.normal_y, npt.normal_z));
pcl::Normal nt;
Eigen::Matrix3f rot = Eigen::Matrix3f::Identity();
rot.row(2) << npt.getNormalVector3fMap().transpose();
Eigen::Vector3f ax2(1,0,0);
if ( npt.getNormalVector3fMap().dot(ax2 ) > 0.1) {
ax2 << 0,1,0;
if ( npt.getNormalVector3fMap().dot(ax2 ) > 0.1) {
ax2 << 0,0,1;
}
}
rot.row(1) << ax2.cross(npt.getNormalVector3fMap()).normalized().transpose();
rot.row(0) = ( ax2 - ax2.dot(npt.getNormalVector3fMap() )*npt.getNormalVector3fMap() ).normalized();
rot = rot*rads->points[idx].radius;
for(int j=0; j<circle_cache.rows(); j++){
Eigen::Vector3f apt = rot*circle_cache.row(j).transpose() + pt.getVector3fMap();
pts->push_back(osg::Vec3(apt[0],apt[1],apt[2]));
npts->push_back(osg::Vec3(npt.normal_x, npt.normal_y, npt.normal_z));
}
geom->addPrimitiveSet( new osg::DrawArrays( GL_TRIANGLE_FAN, pstart, fan_size ) );
pstart+= fan_size;
}
geom->setVertexArray( pts );
geom->setNormalArray(npts);
geom->setNormalBinding( osg::Geometry::BIND_PER_VERTEX );
geom->setStateSet(stateset_);
return geom;
}
///
/// @par Detailed description
/// ...
/// @param [in, out] (param1) ...
/// @return ...
/// @note ...
Eigen::Matrix3f
sasmath::Math::
find_u(sasmol::SasMol &mol, int &frame)
{
Eigen::Matrix3f r ;
float rxx = (mol._x().col(frame)*_x().col(frame)).sum() ;
float rxy = (mol._x().col(frame)*_y().col(frame)).sum() ;
float rxz = (mol._x().col(frame)*_z().col(frame)).sum() ;
float ryx = (mol._y().col(frame)*_x().col(frame)).sum() ;
float ryy = (mol._y().col(frame)*_y().col(frame)).sum() ;
float ryz = (mol._y().col(frame)*_z().col(frame)).sum() ;
float rzx = (mol._z().col(frame)*_x().col(frame)).sum() ;
float rzy = (mol._z().col(frame)*_y().col(frame)).sum() ;
float rzz = (mol._z().col(frame)*_z().col(frame)).sum() ;
r << rxx,rxy,rxz,
ryx,ryy,ryz,
rzx,rzy,rzz;
Eigen::Matrix3f rtr = r.transpose() * r ;
Eigen::SelfAdjointEigenSolver<Eigen::Matrix3f> eigensolver(rtr);
if (eigensolver.info() != Eigen::Success)
{
std::cout << "find_u calculation failed" << std::endl ;
}
Eigen::Matrix<float,3,1> uk ; // eigenvalues
Eigen::Matrix3f ak ; // eigenvectors
uk = eigensolver.eigenvalues() ;
ak = eigensolver.eigenvectors() ;
Eigen::Matrix3f akt = ak.transpose() ;
Eigen::Matrix3f new_ak ;
new_ak.row(0) = akt.row(2) ; //sort eigenvectors
new_ak.row(1) = akt.row(1) ;
new_ak.row(2) = akt.row(0) ;
// python ak0 --> ak[2] ; ak1 --> ak[1] ; ak2 --> ak[0]
//Eigen::Matrix<float,3,1> ak2 = akt.row(2).cross(akt.row(1)) ;
Eigen::MatrixXf rak0 = (r * (akt.row(2).transpose())) ;
Eigen::MatrixXf rak1 = (r * (akt.row(1).transpose())) ;
Eigen::Matrix<float,3,1> urak0 ;
if(uk[2] == 0.0)
{
urak0 = 1E15 * rak0 ;
}
else
{
urak0 = (1.0/sqrt(fabs(uk[2])))*rak0 ;
}
Eigen::Matrix<float,3,1> urak1 ;
if(uk[1] == 0.0)
{
urak1 = 1E15 * rak1 ;
}
else
{
urak1 = (1.0/sqrt(fabs(uk[1])))*rak1 ;
}
Eigen::Matrix<float,3,1> urak2 = urak0.col(0).cross(urak1.col(0)) ;
Eigen::Matrix3f bk ;
bk << urak0,urak1,urak2;
Eigen::Matrix3f u ;
u = (bk * new_ak).transpose() ;
return u ;
/*
u = array([[-0.94513198, 0.31068658, 0.100992 ],
[-0.3006246 , -0.70612572, -0.64110165],
[-0.12786863, -0.63628635, 0.76078203]])
check:
u =
-0.945133 0.310686 0.100992
-0.300624 -0.706126 -0.641102
-0.127869 -0.636287 0.760782
*/
}
template <typename PointInT, typename PointOutT> void
pcl::ROPSEstimation <PointInT, PointOutT>::computeLRF (const PointInT& point, const std::set <unsigned int>& local_triangles, Eigen::Matrix3f& lrf_matrix) const
{
const unsigned int number_of_triangles = static_cast <unsigned int> (local_triangles.size ());
std::vector<Eigen::Matrix3f, Eigen::aligned_allocator<Eigen::Matrix3f> > scatter_matrices (number_of_triangles);
std::vector <float> triangle_area (number_of_triangles);
std::vector <float> distance_weight (number_of_triangles);
float total_area = 0.0f;
const float coeff = 1.0f / 12.0f;
const float coeff_1_div_3 = 1.0f / 3.0f;
Eigen::Vector3f feature_point (point.x, point.y, point.z);
std::set <unsigned int>::const_iterator it;
unsigned int i_triangle = 0;
for (it = local_triangles.begin (), i_triangle = 0; it != local_triangles.end (); it++, i_triangle++)
{
Eigen::Vector3f pt[3];
for (unsigned int i_vertex = 0; i_vertex < 3; i_vertex++)
{
const unsigned int index = triangles_[*it].vertices[i_vertex];
pt[i_vertex] (0) = surface_->points[index].x;
pt[i_vertex] (1) = surface_->points[index].y;
pt[i_vertex] (2) = surface_->points[index].z;
}
const float curr_area = ((pt[1] - pt[0]).cross (pt[2] - pt[0])).norm ();
triangle_area[i_triangle] = curr_area;
total_area += curr_area;
distance_weight[i_triangle] = pow (support_radius_ - (feature_point - (pt[0] + pt[1] + pt[2]) * coeff_1_div_3).norm (), 2.0f);
Eigen::Matrix3f curr_scatter_matrix;
curr_scatter_matrix.setZero ();
for (unsigned int i_pt = 0; i_pt < 3; i_pt++)
{
Eigen::Vector3f vec = pt[i_pt] - feature_point;
curr_scatter_matrix += vec * (vec.transpose ());
for (unsigned int j_pt = 0; j_pt < 3; j_pt++)
curr_scatter_matrix += vec * ((pt[j_pt] - feature_point).transpose ());
}
scatter_matrices[i_triangle] = coeff * curr_scatter_matrix;
}
if (std::abs (total_area) < std::numeric_limits <float>::epsilon ())
total_area = 1.0f / total_area;
else
total_area = 1.0f;
Eigen::Matrix3f overall_scatter_matrix;
overall_scatter_matrix.setZero ();
std::vector<float> total_weight (number_of_triangles);
const float denominator = 1.0f / 6.0f;
for (unsigned int i_triangle = 0; i_triangle < number_of_triangles; i_triangle++)
{
float factor = distance_weight[i_triangle] * triangle_area[i_triangle] * total_area;
overall_scatter_matrix += factor * scatter_matrices[i_triangle];
total_weight[i_triangle] = factor * denominator;
}
Eigen::Vector3f v1, v2, v3;
computeEigenVectors (overall_scatter_matrix, v1, v2, v3);
float h1 = 0.0f;
float h3 = 0.0f;
for (it = local_triangles.begin (), i_triangle = 0; it != local_triangles.end (); it++, i_triangle++)
{
Eigen::Vector3f pt[3];
for (unsigned int i_vertex = 0; i_vertex < 3; i_vertex++)
{
const unsigned int index = triangles_[*it].vertices[i_vertex];
pt[i_vertex] (0) = surface_->points[index].x;
pt[i_vertex] (1) = surface_->points[index].y;
pt[i_vertex] (2) = surface_->points[index].z;
}
float factor1 = 0.0f;
float factor3 = 0.0f;
for (unsigned int i_pt = 0; i_pt < 3; i_pt++)
{
Eigen::Vector3f vec = pt[i_pt] - feature_point;
factor1 += vec.dot (v1);
factor3 += vec.dot (v3);
}
h1 += total_weight[i_triangle] * factor1;
h3 += total_weight[i_triangle] * factor3;
}
if (h1 < 0.0f) v1 = -v1;
if (h3 < 0.0f) v3 = -v3;
v2 = v3.cross (v1);
lrf_matrix.row (0) = v1;
lrf_matrix.row (1) = v2;
lrf_matrix.row (2) = v3;
}
//////////////////////////////////////////////////////////////////////////////////////////////
// Compute a local Reference Frame for a 3D feature; the output is stored in the "rf" matrix
template<typename PointInT, typename PointOutT> float
pcl::SHOTLocalReferenceFrameEstimation<PointInT, PointOutT>::getLocalRF (const int& current_point_idx, Eigen::Matrix3f &rf)
{
const Eigen::Vector4f& central_point = (*input_)[current_point_idx].getVector4fMap ();
std::vector<int> n_indices;
std::vector<float> n_sqr_distances;
searchForNeighbors (current_point_idx, search_parameter_, n_indices, n_sqr_distances);
Eigen::Matrix<double, Eigen::Dynamic, 4> vij (n_indices.size (), 4);
Eigen::Matrix3d cov_m = Eigen::Matrix3d::Zero ();
double distance = 0.0;
double sum = 0.0;
int valid_nn_points = 0;
for (size_t i_idx = 0; i_idx < n_indices.size (); ++i_idx)
{
Eigen::Vector4f pt = surface_->points[n_indices[i_idx]].getVector4fMap ();
if (pt.head<3> () == central_point.head<3> ())
continue;
// Difference between current point and origin
vij.row (valid_nn_points) = (pt - central_point).cast<double> ();
vij (valid_nn_points, 3) = 0;
distance = search_parameter_ - sqrt (n_sqr_distances[i_idx]);
// Multiply vij * vij'
cov_m += distance * (vij.row (valid_nn_points).head<3> ().transpose () * vij.row (valid_nn_points).head<3> ());
sum += distance;
valid_nn_points++;
}
if (valid_nn_points < 5)
{
//PCL_ERROR ("[pcl::%s::getLocalRF] Warning! Neighborhood has less than 5 vertexes. Aborting Local RF computation of feature point (%lf, %lf, %lf)\n", "SHOTLocalReferenceFrameEstimation", central_point[0], central_point[1], central_point[2]);
rf.setConstant (std::numeric_limits<float>::quiet_NaN ());
return (std::numeric_limits<float>::max ());
}
cov_m /= sum;
Eigen::SelfAdjointEigenSolver<Eigen::Matrix3d> solver (cov_m);
const double& e1c = solver.eigenvalues ()[0];
const double& e2c = solver.eigenvalues ()[1];
const double& e3c = solver.eigenvalues ()[2];
if (!pcl_isfinite (e1c) || !pcl_isfinite (e2c) || !pcl_isfinite (e3c))
{
//PCL_ERROR ("[pcl::%s::getLocalRF] Warning! Eigenvectors are NaN. Aborting Local RF computation of feature point (%lf, %lf, %lf)\n", "SHOTLocalReferenceFrameEstimation", central_point[0], central_point[1], central_point[2]);
rf.setConstant (std::numeric_limits<float>::quiet_NaN ());
return (std::numeric_limits<float>::max ());
}
// Disambiguation
Eigen::Vector4d v1 = Eigen::Vector4d::Zero ();
Eigen::Vector4d v3 = Eigen::Vector4d::Zero ();
v1.head<3> () = solver.eigenvectors ().col (2);
v3.head<3> () = solver.eigenvectors ().col (0);
int plusNormal = 0, plusTangentDirection1=0;
for (int ne = 0; ne < valid_nn_points; ne++)
{
double dp = vij.row (ne).dot (v1);
if (dp >= 0)
plusTangentDirection1++;
dp = vij.row (ne).dot (v3);
if (dp >= 0)
plusNormal++;
}
//TANGENT
plusTangentDirection1 = 2*plusTangentDirection1 - valid_nn_points;
if (plusTangentDirection1 == 0)
{
int points = 5; //std::min(valid_nn_points*2/2+1, 11);
int medianIndex = valid_nn_points/2;
for (int i = -points/2; i <= points/2; i++)
if ( vij.row (medianIndex - i).dot (v1) > 0)
plusTangentDirection1 ++;
if (plusTangentDirection1 < points/2+1)
v1 *= - 1;
} else if (plusTangentDirection1 < 0)
v1 *= - 1;
//Normal
plusNormal = 2*plusNormal - valid_nn_points;
if (plusNormal == 0)
{
int points = 5; //std::min(valid_nn_points*2/2+1, 11);
int medianIndex = valid_nn_points/2;
for (int i = -points/2; i <= points/2; i++)
if ( vij.row (medianIndex - i).dot (v3) > 0)
plusNormal ++;
if (plusNormal < points/2+1)
v3 *= - 1;
} else if (plusNormal < 0)
v3 *= - 1;
rf.row (0) = v1.head<3> ().cast<float> ();
rf.row (2) = v3.head<3> ().cast<float> ();
rf.row (1) = rf.row (2).cross (rf.row (0));
return (0.0f);
}
template<typename PointInT, typename PointNT, typename PointOutT> float
pcl::BOARDLocalReferenceFrameEstimation<PointInT, PointNT, PointOutT>::computePointLRF (const int &index,
Eigen::Matrix3f &lrf)
{
//find Z axis
//extract support points for Rz radius
std::vector<int> neighbours_indices;
std::vector<float> neighbours_distances;
int n_neighbours = this->searchForNeighbors (index, search_parameter_, neighbours_indices, neighbours_distances);
//check if there are enough neighbor points, otherwise compute a random X axis and use normal as Z axis
if (n_neighbours < 6)
{
//PCL_WARN(
// "[pcl::%s::computePointLRF] Warning! Neighborhood has less than 6 vertices. Aborting description of point with index %d\n",
// getClassName().c_str(), index);
//setting lrf to NaN
lrf.setConstant (std::numeric_limits<float>::quiet_NaN ());
return (std::numeric_limits<float>::max ());
}
//copy neighbours coordinates into eigen matrix
Eigen::Matrix<float, Eigen::Dynamic, 3> neigh_points_mat (n_neighbours, 3);
for (int i = 0; i < n_neighbours; ++i)
{
neigh_points_mat.row (i) = (*surface_)[neighbours_indices[i]].getVector3fMap ();
}
Eigen::Vector3f x_axis, y_axis;
//plane fitting to find direction of Z axis
Eigen::Vector3f fitted_normal; //z_axis
Eigen::Vector3f centroid;
planeFitting (neigh_points_mat, centroid, fitted_normal);
//disambiguate Z axis with normal mean
normalDisambiguation (*normals_, neighbours_indices, fitted_normal);
//setting LRF Z axis
lrf.row (2).matrix () = fitted_normal;
/////////////////////////////////////////////////////////////////////////////////////////
//find X axis
//extract support points for Rx radius
if (tangent_radius_ != 0.0f && search_parameter_ != tangent_radius_)
{
n_neighbours = this->searchForNeighbors (index, tangent_radius_, neighbours_indices, neighbours_distances);
}
//find point with the "most different" normal (with respect to fittedNormal)
float min_normal_cos = std::numeric_limits<float>::max ();
int min_normal_index = -1;
bool margin_point_found = false;
Eigen::Vector3f best_margin_point;
bool best_point_found_on_margins = false;
float radius2 = tangent_radius_ * tangent_radius_;
float margin_distance2 = margin_thresh_ * margin_thresh_ * radius2;
float max_boundary_angle = 0;
if (find_holes_)
{
randomOrthogonalAxis (fitted_normal, x_axis);
lrf.row (0).matrix () = x_axis;
for (int i = 0; i < check_margin_array_size_; i++)
{
check_margin_array_[i] = false;
margin_array_min_angle_[i] = std::numeric_limits<float>::max ();
margin_array_max_angle_[i] = -std::numeric_limits<float>::max ();
margin_array_min_angle_normal_[i] = -1.0;
margin_array_max_angle_normal_[i] = -1.0;
}
max_boundary_angle = (2 * static_cast<float> (M_PI)) / static_cast<float> (check_margin_array_size_);
}
for (int curr_neigh = 0; curr_neigh < n_neighbours; ++curr_neigh)
{
const int& curr_neigh_idx = neighbours_indices[curr_neigh];
const float& neigh_distance_sqr = neighbours_distances[curr_neigh];
if (neigh_distance_sqr <= margin_distance2)
{
continue;
}
//point normalIndex is inside the ring between marginThresh and Radius
margin_point_found = true;
Eigen::Vector3f normal_mean = normals_->at (curr_neigh_idx).getNormalVector3fMap ();
float normal_cos = fitted_normal.dot (normal_mean);
if (normal_cos < min_normal_cos)
{
min_normal_index = curr_neigh_idx;
min_normal_cos = normal_cos;
best_point_found_on_margins = false;
}
if (find_holes_)
{
//find angle with respect to random axis previously calculated
Eigen::Vector3f indicating_normal_vect;
directedOrthogonalAxis (fitted_normal, input_->at (index).getVector3fMap (),
surface_->at (curr_neigh_idx).getVector3fMap (), indicating_normal_vect);
float angle = getAngleBetweenUnitVectors (x_axis, indicating_normal_vect, fitted_normal);
int check_margin_array_idx = std::min (static_cast<int> (floor (angle / max_boundary_angle)), check_margin_array_size_ - 1);
check_margin_array_[check_margin_array_idx] = true;
if (angle < margin_array_min_angle_[check_margin_array_idx])
{
margin_array_min_angle_[check_margin_array_idx] = angle;
margin_array_min_angle_normal_[check_margin_array_idx] = normal_cos;
}
if (angle > margin_array_max_angle_[check_margin_array_idx])
{
margin_array_max_angle_[check_margin_array_idx] = angle;
margin_array_max_angle_normal_[check_margin_array_idx] = normal_cos;
}
}
} //for each neighbor
if (!margin_point_found)
{
//find among points with neighDistance <= marginThresh*radius
for (int curr_neigh = 0; curr_neigh < n_neighbours; curr_neigh++)
{
const int& curr_neigh_idx = neighbours_indices[curr_neigh];
const float& neigh_distance_sqr = neighbours_distances[curr_neigh];
if (neigh_distance_sqr > margin_distance2)
continue;
Eigen::Vector3f normal_mean = normals_->at (curr_neigh_idx).getNormalVector3fMap ();
float normal_cos = fitted_normal.dot (normal_mean);
if (normal_cos < min_normal_cos)
{
min_normal_index = curr_neigh_idx;
min_normal_cos = normal_cos;
}
}//for each neighbor
// Check if we are not in a degenerate case (all the neighboring normals are NaNs)
if (min_normal_index == -1)
{
lrf.setConstant (std::numeric_limits<float>::quiet_NaN ());
return (std::numeric_limits<float>::max ());
}
//find orthogonal axis directed to minNormalIndex point projection on plane with fittedNormal as axis
directedOrthogonalAxis (fitted_normal, input_->at (index).getVector3fMap (),
surface_->at (min_normal_index).getVector3fMap (), x_axis);
y_axis = fitted_normal.cross (x_axis);
lrf.row (0).matrix () = x_axis;
lrf.row (1).matrix () = y_axis;
//z axis already set
return (min_normal_cos);
}
if (!find_holes_)
{
if (best_point_found_on_margins)
{
//if most inclined normal is on support margin
directedOrthogonalAxis (fitted_normal, input_->at (index).getVector3fMap (), best_margin_point, x_axis);
y_axis = fitted_normal.cross (x_axis);
lrf.row (0).matrix () = x_axis;
lrf.row (1).matrix () = y_axis;
//z axis already set
return (min_normal_cos);
}
// Check if we are not in a degenerate case (all the neighboring normals are NaNs)
if (min_normal_index == -1)
{
lrf.setConstant (std::numeric_limits<float>::quiet_NaN ());
return (std::numeric_limits<float>::max ());
}
directedOrthogonalAxis (fitted_normal, input_->at (index).getVector3fMap (),
surface_->at (min_normal_index).getVector3fMap (), x_axis);
y_axis = fitted_normal.cross (x_axis);
lrf.row (0).matrix () = x_axis;
lrf.row (1).matrix () = y_axis;
//z axis already set
return (min_normal_cos);
}// if(!find_holes_)
//check if there is at least a hole
bool is_hole_present = false;
for (int i = 0; i < check_margin_array_size_; i++)
{
if (!check_margin_array_[i])
{
is_hole_present = true;
break;
}
}
if (!is_hole_present)
{
if (best_point_found_on_margins)
{
//if most inclined normal is on support margin
directedOrthogonalAxis (fitted_normal, input_->at (index).getVector3fMap (), best_margin_point, x_axis);
y_axis = fitted_normal.cross (x_axis);
lrf.row (0).matrix () = x_axis;
lrf.row (1).matrix () = y_axis;
//z axis already set
return (min_normal_cos);
}
// Check if we are not in a degenerate case (all the neighboring normals are NaNs)
if (min_normal_index == -1)
{
lrf.setConstant (std::numeric_limits<float>::quiet_NaN ());
return (std::numeric_limits<float>::max ());
}
//find orthogonal axis directed to minNormalIndex point projection on plane with fittedNormal as axis
directedOrthogonalAxis (fitted_normal, input_->at (index).getVector3fMap (),
surface_->at (min_normal_index).getVector3fMap (), x_axis);
y_axis = fitted_normal.cross (x_axis);
lrf.row (0).matrix () = x_axis;
lrf.row (1).matrix () = y_axis;
//z axis already set
return (min_normal_cos);
}//if (!is_hole_present)
//case hole found
//find missing region
float angle = 0.0;
int hole_end;
int hole_first;
//find first no border pie
int first_no_border = -1;
if (check_margin_array_[check_margin_array_size_ - 1])
{
first_no_border = 0;
}
else
{
for (int i = 0; i < check_margin_array_size_; i++)
{
if (check_margin_array_[i])
{
first_no_border = i;
break;
}
}
}
//float steep_prob = 0.0;
float max_hole_prob = -std::numeric_limits<float>::max ();
//find holes
for (int ch = first_no_border; ch < check_margin_array_size_; ch++)
{
if (!check_margin_array_[ch])
{
//border beginning found
hole_first = ch;
hole_end = hole_first + 1;
while (!check_margin_array_[hole_end % check_margin_array_size_])
{
++hole_end;
}
//border end found, find angle
if ((hole_end - hole_first) > 0)
{
//check if hole can be a shapeness hole
int previous_hole = (((hole_first - 1) < 0) ? (hole_first - 1) + check_margin_array_size_ : (hole_first - 1))
% check_margin_array_size_;
int following_hole = (hole_end) % check_margin_array_size_;
float normal_begin = margin_array_max_angle_normal_[previous_hole];
float normal_end = margin_array_min_angle_normal_[following_hole];
normal_begin -= min_normal_cos;
normal_end -= min_normal_cos;
normal_begin = normal_begin / (1.0f - min_normal_cos);
normal_end = normal_end / (1.0f - min_normal_cos);
normal_begin = 1.0f - normal_begin;
normal_end = 1.0f - normal_end;
//evaluate P(Hole);
float hole_width = 0.0f;
if (following_hole < previous_hole)
{
hole_width = margin_array_min_angle_[following_hole] + 2 * static_cast<float> (M_PI)
- margin_array_max_angle_[previous_hole];
}
else
{
hole_width = margin_array_min_angle_[following_hole] - margin_array_max_angle_[previous_hole];
}
float hole_prob = hole_width / (2 * static_cast<float> (M_PI));
//evaluate P(zmin|Hole)
float steep_prob = (normal_end + normal_begin) / 2.0f;
//check hole prob and after that, check steepThresh
if (hole_prob > hole_size_prob_thresh_)
{
if (steep_prob > steep_thresh_)
{
if (hole_prob > max_hole_prob)
{
max_hole_prob = hole_prob;
float angle_weight = ((normal_end - normal_begin) + 1.0f) / 2.0f;
if (following_hole < previous_hole)
{
angle = margin_array_max_angle_[previous_hole] + (margin_array_min_angle_[following_hole] + 2
* static_cast<float> (M_PI) - margin_array_max_angle_[previous_hole]) * angle_weight;
}
else
{
angle = margin_array_max_angle_[previous_hole] + (margin_array_min_angle_[following_hole]
- margin_array_max_angle_[previous_hole]) * angle_weight;
}
}
}
}
} //(hole_end-hole_first) > 0
if (hole_end >= check_margin_array_size_)
{
break;
}
else
{
ch = hole_end - 1;
}
}
}
if (max_hole_prob > -std::numeric_limits<float>::max ())
{
//hole found
Eigen::AngleAxisf rotation = Eigen::AngleAxisf (angle, fitted_normal);
x_axis = rotation * x_axis;
min_normal_cos -= 10.0f;
}
else
{
if (best_point_found_on_margins)
{
//if most inclined normal is on support margin
directedOrthogonalAxis (fitted_normal, input_->at (index).getVector3fMap (), best_margin_point, x_axis);
}
else
{
// Check if we are not in a degenerate case (all the neighboring normals are NaNs)
if (min_normal_index == -1)
{
lrf.setConstant (std::numeric_limits<float>::quiet_NaN ());
return (std::numeric_limits<float>::max ());
}
//find orthogonal axis directed to minNormalIndex point projection on plane with fittedNormal as axis
directedOrthogonalAxis (fitted_normal, input_->at (index).getVector3fMap (),
surface_->at (min_normal_index).getVector3fMap (), x_axis);
}
}
y_axis = fitted_normal.cross (x_axis);
lrf.row (0).matrix () = x_axis;
lrf.row (1).matrix () = y_axis;
//z axis already set
return (min_normal_cos);
}
void SensorFusion::normalize()
{
float error = 0;
Eigen::Matrix3f temporary;
float renorm = 0;
error = -dcmMatrix.row(0).dot(dcmMatrix.row(1)) * 0.5f; //eq.19
temporary.row(0) = dcmMatrix.row(1) * error;
temporary.row(1) = dcmMatrix.row(0) * error;
temporary.row(0) += dcmMatrix.row(0);
temporary.row(1) += dcmMatrix.row(1);
temporary.row(2) = temporary.row(0).cross(temporary.row(1)); // c= a x b //eq.20
renorm = 0.5f *(3.0f - temporary.row(0).dot(temporary.row(0))); //eq.21
dcmMatrix.row(0) = temporary.row(0) * renorm;
renorm = 0.5f *(3.0f - temporary.row(1).dot(temporary.row(1))); //eq.21
dcmMatrix.row(1) = temporary.row(1) * renorm;
renorm = 0.5f *(3.0f - temporary.row(2).dot(temporary.row(2))); //eq.21
dcmMatrix.row(2) = temporary.row(2) * renorm;
}
void get3DMoments(vector<Point> feature_points, int feat_index, int index)
{
// for(int i=0; i<feature_points.size(); i++)
// ROS_INFO("%d --> %d,%d",i,feature_points[i].x,feature_points[i].y);
//ROS_INFO("Getting 3D Moments : %d --> %d,%d", feature_points.size(), width, height);
//Extract the indices for the points in the point cloud data
pcl::PointIndices point_indices;
for(int i=0; i<feature_points.size(); i++)
{
//ROS_INFO("Feature Index : %d, %d",feature_points[i].x, feature_points[i].y);
point_indices.indices.push_back(feature_points[i].y * width + feature_points[i].x);
}
//ROS_INFO("Computing 3D Centroid : %d",point_indices.indices.size());
Eigen::Vector4f centroid;
Eigen::Matrix3f covariance_matrix;
// Estimate the XYZ centroid
pcl::compute3DCentroid (pcl_in, point_indices, centroid);
#ifdef DEBUG
ROS_INFO("Centroid %d: %f, %f, %f, %f",index,centroid(0),centroid(1),centroid(2),centroid(3));
#endif
//ROS_INFO("Computing Covariance ");
//Compute the centroid and the covariance of the points
computeCovarianceMatrix(pcl_in, point_indices.indices, centroid, covariance_matrix);
//Print the 3D Moments
//ROS_INFO("Centroid : %f, %f, %f, %f",centroid(0),centroid(1),centroid(2),centroid(3));
#ifdef DEBUG
std::cout<<"Covariance : "<<std::endl<<covariance_matrix <<std::endl;
#endif
for(int i=0; i<3; i++)
{
feedback_.features[feat_index].moments[index].mean[i] = centroid(i);
for(int j=0; j<3; j++)
{
feedback_.features[feat_index].moments[index].covariance[i*3+j] = covariance_matrix(i,j);
}
}
//Get the principal components and the quaternion
Eigen::Matrix3f evecs;
Eigen::Vector3f evals;
pcl::eigen33 (covariance_matrix, evecs, evals);
Eigen::Matrix3f rotation;
rotation.row (0) = evecs.col (0);
rotation.row (1) = evecs.col (1);
//rotation.row (2) = evecs.col (2);
rotation.row (2) = rotation.row (0).cross (rotation.row (1));
//rotation.transposeInPlace ();
#ifdef DEBUG
std::cerr << "Rotation matrix: " << endl;
std::cerr << rotation << endl;
std::cout<<"Eigen vals : "<<evals<<std::endl;
#endif
rotation.transposeInPlace ();
Eigen::Quaternion<float> qt (rotation);
qt.normalize ();
//Publish Marker
visualization_msgs::Marker marker;
marker.header.frame_id = "/openni_rgb_optical_frame";
marker.header.stamp = ros::Time().now();
marker.ns = "Triangulation";
marker.id = index+1;
marker.action = visualization_msgs::Marker::ADD;
marker.lifetime = ros::Duration(5);
//centroid position
marker.type = visualization_msgs::Marker::SPHERE;
marker.pose.position.x = centroid(0);
marker.pose.position.y = centroid(1);
marker.pose.position.z = centroid(2);
marker.pose.orientation.x = qt.x();
marker.pose.orientation.y = qt.y();
marker.pose.orientation.z = qt.z();
marker.pose.orientation.w = qt.w();
marker.scale.x = sqrt(evals(0)) *2;
marker.scale.y = sqrt(evals(1)) *2;
marker.scale.z = sqrt(evals(2)) *2;
//red
marker.color.a = 0.5;
marker.color.r = rand()/((double)RAND_MAX + 1);
marker.color.g = rand()/((double)RAND_MAX + 1);
marker.color.b = rand()/((double)RAND_MAX + 1);
object_pose_marker_pub_.publish(marker);
}
