inline vcl_size_t size(Eigen::VectorXf const & v) { return v.rows(); }
template <typename PointInT, typename PointNT, typename PointOutT> void
pcl::PFHEstimation<PointInT, PointNT, PointOutT>::computePointPFHSignature (
const pcl::PointCloud<PointInT> &cloud, const pcl::PointCloud<PointNT> &normals,
const std::vector<int> &indices, int nr_split, Eigen::VectorXf &pfh_histogram)
{
int h_index, h_p;
// Clear the resultant point histogram
pfh_histogram.setZero ();
// Factorization constant
float hist_incr = 100.0f / static_cast<float> (indices.size () * (indices.size () - 1) / 2);
std::pair<int, int> key;
// Iterate over all the points in the neighborhood
for (size_t i_idx = 0; i_idx < indices.size (); ++i_idx)
{
for (size_t j_idx = 0; j_idx < i_idx; ++j_idx)
{
// If the 3D points are invalid, don't bother estimating, just continue
if (!isFinite (cloud.points[indices[i_idx]]) || !isFinite (cloud.points[indices[j_idx]]))
continue;
if (use_cache_)
{
// In order to create the key, always use the smaller index as the first key pair member
int p1, p2;
// if (indices[i_idx] >= indices[j_idx])
// {
p1 = indices[i_idx];
p2 = indices[j_idx];
// }
// else
// {
// p1 = indices[j_idx];
// p2 = indices[i_idx];
// }
key = std::pair<int, int> (p1, p2);
// Check to see if we already estimated this pair in the global hashmap
std::map<std::pair<int, int>, Eigen::Vector4f, std::less<std::pair<int, int> >, Eigen::aligned_allocator<Eigen::Vector4f> >::iterator fm_it = feature_map_.find (key);
if (fm_it != feature_map_.end ())
pfh_tuple_ = fm_it->second;
else
{
// Compute the pair NNi to NNj
if (!computePairFeatures (cloud, normals, indices[i_idx], indices[j_idx],
pfh_tuple_[0], pfh_tuple_[1], pfh_tuple_[2], pfh_tuple_[3]))
continue;
}
}
else
if (!computePairFeatures (cloud, normals, indices[i_idx], indices[j_idx],
pfh_tuple_[0], pfh_tuple_[1], pfh_tuple_[2], pfh_tuple_[3]))
continue;
// Normalize the f1, f2, f3 features and push them in the histogram
f_index_[0] = static_cast<int> (floor (nr_split * ((pfh_tuple_[0] + M_PI) * d_pi_)));
if (f_index_[0] < 0) f_index_[0] = 0;
if (f_index_[0] >= nr_split) f_index_[0] = nr_split - 1;
f_index_[1] = static_cast<int> (floor (nr_split * ((pfh_tuple_[1] + 1.0) * 0.5)));
if (f_index_[1] < 0) f_index_[1] = 0;
if (f_index_[1] >= nr_split) f_index_[1] = nr_split - 1;
f_index_[2] = static_cast<int> (floor (nr_split * ((pfh_tuple_[2] + 1.0) * 0.5)));
if (f_index_[2] < 0) f_index_[2] = 0;
if (f_index_[2] >= nr_split) f_index_[2] = nr_split - 1;
// Copy into the histogram
h_index = 0;
h_p = 1;
for (int d = 0; d < 3; ++d)
{
h_index += h_p * f_index_[d];
h_p *= nr_split;
}
pfh_histogram[h_index] += hist_incr;
if (use_cache_)
{
// Save the value in the hashmap
feature_map_[key] = pfh_tuple_;
// Use a maximum cache so that we don't go overboard on RAM usage
key_list_.push (key);
// Check to see if we need to remove an element due to exceeding max_size
if (key_list_.size () > max_cache_size_)
{
// Remove the last element.
feature_map_.erase (key_list_.back ());
key_list_.pop ();
}
}
}
}
}
template <typename PointT, typename PointNT> bool
pcl::SampleConsensusModelCone<PointT, PointNT>::computeModelCoefficients (
const std::vector<int> &samples, Eigen::VectorXf &model_coefficients)
{
// Need 3 samples
if (samples.size () != 3)
{
PCL_ERROR ("[pcl::SampleConsensusModelCone::computeModelCoefficients] Invalid set of samples given (%lu)!\n", samples.size ());
return (false);
}
if (!normals_)
{
PCL_ERROR ("[pcl::SampleConsensusModelCone::computeModelCoefficients] No input dataset containing normals was given!\n");
return (false);
}
Eigen::Vector4f p1 (input_->points[samples[0]].x, input_->points[samples[0]].y, input_->points[samples[0]].z, 0);
Eigen::Vector4f p2 (input_->points[samples[1]].x, input_->points[samples[1]].y, input_->points[samples[1]].z, 0);
Eigen::Vector4f p3 (input_->points[samples[2]].x, input_->points[samples[2]].y, input_->points[samples[2]].z, 0);
Eigen::Vector4f n1 (normals_->points[samples[0]].normal[0], normals_->points[samples[0]].normal[1], normals_->points[samples[0]].normal[2], 0);
Eigen::Vector4f n2 (normals_->points[samples[1]].normal[0], normals_->points[samples[1]].normal[1], normals_->points[samples[1]].normal[2], 0);
Eigen::Vector4f n3 (normals_->points[samples[2]].normal[0], normals_->points[samples[2]].normal[1], normals_->points[samples[2]].normal[2], 0);
//calculate apex (intersection of the three planes defined by points and belonging normals
Eigen::Vector4f ortho12 = n1.cross3(n2);
Eigen::Vector4f ortho23 = n2.cross3(n3);
Eigen::Vector4f ortho31 = n3.cross3(n1);
float denominator = n1.dot(ortho23);
float d1 = p1.dot (n1);
float d2 = p2.dot (n2);
float d3 = p3.dot (n3);
Eigen::Vector4f apex = (d1 * ortho23 + d2 * ortho31 + d3 * ortho12) / denominator;
//compute axis (normal of plane defined by: { apex+(p1-apex)/(||p1-apex||), apex+(p2-apex)/(||p2-apex||), apex+(p3-apex)/(||p3-apex||)}
Eigen::Vector4f ap1 = p1 - apex;
Eigen::Vector4f ap2 = p2 - apex;
Eigen::Vector4f ap3 = p3 - apex;
Eigen::Vector4f np1 = apex + (ap1/ap1.norm ());
Eigen::Vector4f np2 = apex + (ap2/ap2.norm ());
Eigen::Vector4f np3 = apex + (ap3/ap3.norm ());
Eigen::Vector4f np1np2 = np2 - np1;
Eigen::Vector4f np1np3 = np3 - np1;
Eigen::Vector4f axis_dir = np1np2.cross3 (np1np3);
axis_dir.normalize ();
// normalize the vector (apex->p) for opening angle calculation
ap1.normalize ();
ap2.normalize ();
ap3.normalize ();
//compute opening angle
float opening_angle = ( acosf (ap1.dot (axis_dir)) + acosf (ap2.dot (axis_dir)) + acosf (ap3.dot (axis_dir)) ) / 3.0f;
model_coefficients.resize (7);
// model_coefficients.template head<3> () = line_pt.template head<3> ();
model_coefficients[0] = apex[0];
model_coefficients[1] = apex[1];
model_coefficients[2] = apex[2];
// model_coefficients.template segment<3> (3) = line_dir.template head<3> ();
model_coefficients[3] = axis_dir[0];
model_coefficients[4] = axis_dir[1];
model_coefficients[5] = axis_dir[2];
// cone radius
model_coefficients[6] = opening_angle;
if (model_coefficients[6] != -std::numeric_limits<double>::max() && model_coefficients[6] < min_angle_)
return (false);
if (model_coefficients[6] != std::numeric_limits<double>::max() && model_coefficients[6] > max_angle_)
return (false);
return (true);
}
TEST (RANSAC, SampleConsensusModelCircle3D)
{
srand (0);
// Use a custom point cloud for these tests until we need something better
PointCloud<PointXYZ> cloud;
cloud.points.resize (20);
cloud.points[0].x = 1.0f; cloud.points[0].y = 5.0f; cloud.points[0].z = -2.9000001f;
cloud.points[1].x = 1.034202f; cloud.points[1].y = 5.0f; cloud.points[1].z = -2.9060307f;
cloud.points[2].x = 1.0642787f; cloud.points[2].y = 5.0f; cloud.points[2].z = -2.9233956f;
cloud.points[3].x = 1.0866026f; cloud.points[3].y = 5.0f; cloud.points[3].z = -2.95f;
cloud.points[4].x = 1.0984808f; cloud.points[4].y = 5.0f; cloud.points[4].z = -2.9826353f;
cloud.points[5].x = 1.0984808f; cloud.points[5].y = 5.0f; cloud.points[5].z = -3.0173647f;
cloud.points[6].x = 1.0866026f; cloud.points[6].y = 5.0f; cloud.points[6].z = -3.05f;
cloud.points[7].x = 1.0642787f; cloud.points[7].y = 5.0f; cloud.points[7].z = -3.0766044f;
cloud.points[8].x = 1.034202f; cloud.points[8].y = 5.0f; cloud.points[8].z = -3.0939693f;
cloud.points[9].x = 1.0f; cloud.points[9].y = 5.0f; cloud.points[9].z = -3.0999999f;
cloud.points[10].x = 0.96579796f; cloud.points[10].y = 5.0f; cloud.points[10].z = -3.0939693f;
cloud.points[11].x = 0.93572122f; cloud.points[11].y = 5.0f; cloud.points[11].z = -3.0766044f;
cloud.points[12].x = 0.91339743f; cloud.points[12].y = 5.0f; cloud.points[12].z = -3.05f;
cloud.points[13].x = 0.90151924f; cloud.points[13].y = 5.0f; cloud.points[13].z = -3.0173647f;
cloud.points[14].x = 0.90151924f; cloud.points[14].y = 5.0f; cloud.points[14].z = -2.9826353f;
cloud.points[15].x = 0.91339743f; cloud.points[15].y = 5.0f; cloud.points[15].z = -2.95f;
cloud.points[16].x = 0.93572122f; cloud.points[16].y = 5.0f; cloud.points[16].z = -2.9233956f;
cloud.points[17].x = 0.96579796f; cloud.points[17].y = 5.0; cloud.points[17].z = -2.9060307f;
cloud.points[18].x = 0.85000002f; cloud.points[18].y = 4.8499999f; cloud.points[18].z = -3.1500001f;
cloud.points[19].x = 1.15f; cloud.points[19].y = 5.1500001f; cloud.points[19].z = -2.8499999f;
// Create a shared 3d circle model pointer directly
SampleConsensusModelCircle3DPtr model (new SampleConsensusModelCircle3D<PointXYZ> (cloud.makeShared ()));
// Create the RANSAC object
RandomSampleConsensus<PointXYZ> sac (model, 0.03);
// Algorithm tests
bool result = sac.computeModel ();
ASSERT_EQ (result, true);
std::vector<int> sample;
sac.getModel (sample);
EXPECT_EQ (int (sample.size ()), 3);
std::vector<int> inliers;
sac.getInliers (inliers);
EXPECT_EQ (int (inliers.size ()), 18);
Eigen::VectorXf coeff;
sac.getModelCoefficients (coeff);
EXPECT_EQ (int (coeff.size ()), 7);
EXPECT_NEAR (coeff[0], 1, 1e-3);
EXPECT_NEAR (coeff[1], 5, 1e-3);
EXPECT_NEAR (coeff[2], -3, 1e-3);
EXPECT_NEAR (coeff[3],0.1, 1e-3);
EXPECT_NEAR (coeff[4], 0, 1e-3);
EXPECT_NEAR (coeff[5], -1, 1e-3);
EXPECT_NEAR (coeff[6], 0, 1e-3);
Eigen::VectorXf coeff_refined;
model->optimizeModelCoefficients (inliers, coeff, coeff_refined);
EXPECT_EQ (int (coeff_refined.size ()), 7);
EXPECT_NEAR (coeff_refined[0], 1, 1e-3);
EXPECT_NEAR (coeff_refined[1], 5, 1e-3);
EXPECT_NEAR (coeff_refined[2], -3, 1e-3);
EXPECT_NEAR (coeff_refined[3],0.1, 1e-3);
EXPECT_NEAR (coeff_refined[4], 0, 1e-3);
EXPECT_NEAR (coeff_refined[5], -1, 1e-3);
EXPECT_NEAR (coeff_refined[6], 0, 1e-3);
}
Eigen::MatrixXf featureGradient( const Eigen::MatrixXf & a, const Eigen::MatrixXf & b ) const {
if (ntype_ == NO_NORMALIZATION )
return kernelGradient( a, b );
else if (ntype_ == NORMALIZE_SYMMETRIC ) {
Eigen::MatrixXf fa = lattice_.compute( a*norm_.asDiagonal(), true );
Eigen::MatrixXf fb = lattice_.compute( b*norm_.asDiagonal() );
Eigen::MatrixXf ones = Eigen::MatrixXf::Ones( a.rows(), a.cols() );
Eigen::VectorXf norm3 = norm_.array()*norm_.array()*norm_.array();
Eigen::MatrixXf r = kernelGradient( 0.5*( a.array()*fb.array() + fa.array()*b.array() ).matrix()*norm3.asDiagonal(), ones );
return - r + kernelGradient( a*norm_.asDiagonal(), b*norm_.asDiagonal() );
}
else if (ntype_ == NORMALIZE_AFTER ) {
Eigen::MatrixXf fb = lattice_.compute( b );
Eigen::MatrixXf ones = Eigen::MatrixXf::Ones( a.rows(), a.cols() );
Eigen::VectorXf norm2 = norm_.array()*norm_.array();
Eigen::MatrixXf r = kernelGradient( ( a.array()*fb.array() ).matrix()*norm2.asDiagonal(), ones );
return - r + kernelGradient( a*norm_.asDiagonal(), b );
}
else /*if (ntype_ == NORMALIZE_BEFORE )*/ {
Eigen::MatrixXf fa = lattice_.compute( a, true );
Eigen::MatrixXf ones = Eigen::MatrixXf::Ones( a.rows(), a.cols() );
Eigen::VectorXf norm2 = norm_.array()*norm_.array();
Eigen::MatrixXf r = kernelGradient( ( fa.array()*b.array() ).matrix()*norm2.asDiagonal(), ones );
return -r+kernelGradient( a, b*norm_.asDiagonal() );
}
}
bool
ObjectModelPlaneFromLines::computeModelCoefficients (const std::vector<int> &sample,
Eigen::VectorXf &modelCoefficients){
line line1,line2;
//Create line1 from sample[0] and sample[1]
line1.pbase = (*inputPointCloud->getPointCloud())[sample[0]];
line1.director.resize(3);
line1.director[0]=(*inputPointCloud->getPointCloud())[sample[1]].getX() - (*inputPointCloud->getPointCloud())[sample[0]].getX();
line1.director[1]=(*inputPointCloud->getPointCloud())[sample[1]].getY() - (*inputPointCloud->getPointCloud())[sample[0]].getY();
line1.director[2]=(*inputPointCloud->getPointCloud())[sample[1]].getZ() - (*inputPointCloud->getPointCloud())[sample[0]].getZ();
//Create line1 from sample[2] and sample[3]
line2.pbase = (*inputPointCloud->getPointCloud())[sample[2]];
line2.director.resize(3);
line2.director[0]=(*inputPointCloud->getPointCloud())[sample[3]].getX() - (*inputPointCloud->getPointCloud())[sample[2]].getX();
line2.director[1]=(*inputPointCloud->getPointCloud())[sample[3]].getY() - (*inputPointCloud->getPointCloud())[sample[2]].getY();
line2.director[2]=(*inputPointCloud->getPointCloud())[sample[3]].getZ() - (*inputPointCloud->getPointCloud())[sample[2]].getZ();
//Compute the model co-efficient of the lines from these lines
std::vector<double> normal;
normal.resize(3);
crossProduct3D(line1.director,line2.director,normal);
modelCoefficients.resize(4);
modelCoefficients[0] = normal[0];
modelCoefficients[1] = normal[1];
modelCoefficients[2] = normal[2];
modelCoefficients[3]= -modelCoefficients[0] * line1.pbase.getX() -
modelCoefficients[1] * line1.pbase.getY() -
modelCoefficients[2] * line1.pbase.getZ();
if (abs(modelCoefficients[0])<geometryEpsilon&&abs(modelCoefficients[1])<geometryEpsilon&&abs(modelCoefficients[2])<geometryEpsilon) {
//Lines are parallel
if (contains(line1, line2.pbase))
return false;
//Use a line's director vector and both pBase's difference to create the plane.
std::vector<double> baseDifference;
baseDifference.resize(3);
baseDifference[0]=line1.pbase.getX()-line2.pbase.getX();
baseDifference[1]=line1.pbase.getY()-line2.pbase.getY();
baseDifference[2]=line1.pbase.getZ()-line2.pbase.getZ();
crossProduct3D(line1.director,baseDifference,normal);
modelCoefficients[0] = normal[0];
modelCoefficients[1] = normal[1];
modelCoefficients[2] = normal[2];
modelCoefficients[3]=-modelCoefficients[0] * line1.pbase.getX() -modelCoefficients[1] * line1.pbase.getY() -modelCoefficients[2] * line1.pbase.getZ();
} else {
double x = modelCoefficients[0]*line2.pbase.getX()+
modelCoefficients[1]*line2.pbase.getY()+
modelCoefficients[2]*line2.pbase.getZ()+modelCoefficients[3];
if (abs(x)>=geometryEpsilon) {
cout<<"Lines do not intersect"<<endl;
return false;
}
}
return true;
}
TEST (RANSAC, SampleConsensusModelCone)
{
srand (0);
// Use a custom point cloud for these tests until we need something better
PointCloud<PointXYZ> cloud;
PointCloud<Normal> normals;
cloud.points.resize (31); normals.points.resize (31);
cloud.points[0].x = -0.011247f; cloud.points[0].y = 0.200000f; cloud.points[0].z = 0.965384f;
cloud.points[1].x = 0.000000f; cloud.points[1].y = 0.200000f; cloud.points[1].z = 0.963603f;
cloud.points[2].x = 0.011247f; cloud.points[2].y = 0.200000f; cloud.points[2].z = 0.965384f;
cloud.points[3].x = -0.016045f; cloud.points[3].y = 0.175000f; cloud.points[3].z = 0.977916f;
cloud.points[4].x = -0.008435f; cloud.points[4].y = 0.175000f; cloud.points[4].z = 0.974038f;
cloud.points[5].x = 0.004218f; cloud.points[5].y = 0.175000f; cloud.points[5].z = 0.973370f;
cloud.points[6].x = 0.016045f; cloud.points[6].y = 0.175000f; cloud.points[6].z = 0.977916f;
cloud.points[7].x = -0.025420f; cloud.points[7].y = 0.200000f; cloud.points[7].z = 0.974580f;
cloud.points[8].x = 0.025420f; cloud.points[8].y = 0.200000f; cloud.points[8].z = 0.974580f;
cloud.points[9].x = -0.012710f; cloud.points[9].y = 0.150000f; cloud.points[9].z = 0.987290f;
cloud.points[10].x = -0.005624f; cloud.points[10].y = 0.150000f; cloud.points[10].z = 0.982692f;
cloud.points[11].x = 0.002812f; cloud.points[11].y = 0.150000f; cloud.points[11].z = 0.982247f;
cloud.points[12].x = 0.012710f; cloud.points[12].y = 0.150000f; cloud.points[12].z = 0.987290f;
cloud.points[13].x = -0.022084f; cloud.points[13].y = 0.175000f; cloud.points[13].z = 0.983955f;
cloud.points[14].x = 0.022084f; cloud.points[14].y = 0.175000f; cloud.points[14].z = 0.983955f;
cloud.points[15].x = -0.034616f; cloud.points[15].y = 0.200000f; cloud.points[15].z = 0.988753f;
cloud.points[16].x = 0.034616f; cloud.points[16].y = 0.200000f; cloud.points[16].z = 0.988753f;
cloud.points[17].x = -0.006044f; cloud.points[17].y = 0.125000f; cloud.points[17].z = 0.993956f;
cloud.points[18].x = 0.004835f; cloud.points[18].y = 0.125000f; cloud.points[18].z = 0.993345f;
cloud.points[19].x = -0.017308f; cloud.points[19].y = 0.150000f; cloud.points[19].z = 0.994376f;
cloud.points[20].x = 0.017308f; cloud.points[20].y = 0.150000f; cloud.points[20].z = 0.994376f;
cloud.points[21].x = -0.025962f; cloud.points[21].y = 0.175000f; cloud.points[21].z = 0.991565f;
cloud.points[22].x = 0.025962f; cloud.points[22].y = 0.175000f; cloud.points[22].z = 0.991565f;
cloud.points[23].x = -0.009099f; cloud.points[23].y = 0.125000f; cloud.points[23].z = 1.000000f;
cloud.points[24].x = 0.009099f; cloud.points[24].y = 0.125000f; cloud.points[24].z = 1.000000f;
cloud.points[25].x = -0.018199f; cloud.points[25].y = 0.150000f; cloud.points[25].z = 1.000000f;
cloud.points[26].x = 0.018199f; cloud.points[26].y = 0.150000f; cloud.points[26].z = 1.000000f;
cloud.points[27].x = -0.027298f; cloud.points[27].y = 0.175000f; cloud.points[27].z = 1.000000f;
cloud.points[28].x = 0.027298f; cloud.points[28].y = 0.175000f; cloud.points[28].z = 1.000000f;
cloud.points[29].x = -0.036397f; cloud.points[29].y = 0.200000f; cloud.points[29].z = 1.000000f;
cloud.points[30].x = 0.036397f; cloud.points[30].y = 0.200000f; cloud.points[30].z = 1.000000f;
normals.points[0].normal[0] = -0.290381f; normals.points[0].normal[1] = -0.342020f; normals.points[0].normal[2] = -0.893701f;
normals.points[1].normal[0] = 0.000000f; normals.points[1].normal[1] = -0.342020f; normals.points[1].normal[2] = -0.939693f;
normals.points[2].normal[0] = 0.290381f; normals.points[2].normal[1] = -0.342020f; normals.points[2].normal[2] = -0.893701f;
normals.points[3].normal[0] = -0.552338f; normals.points[3].normal[1] = -0.342020f; normals.points[3].normal[2] = -0.760227f;
normals.points[4].normal[0] = -0.290381f; normals.points[4].normal[1] = -0.342020f; normals.points[4].normal[2] = -0.893701f;
normals.points[5].normal[0] = 0.145191f; normals.points[5].normal[1] = -0.342020f; normals.points[5].normal[2] = -0.916697f;
normals.points[6].normal[0] = 0.552337f; normals.points[6].normal[1] = -0.342020f; normals.points[6].normal[2] = -0.760227f;
normals.points[7].normal[0] = -0.656282f; normals.points[7].normal[1] = -0.342020f; normals.points[7].normal[2] = -0.656283f;
normals.points[8].normal[0] = 0.656282f; normals.points[8].normal[1] = -0.342020f; normals.points[8].normal[2] = -0.656283f;
normals.points[9].normal[0] = -0.656283f; normals.points[9].normal[1] = -0.342020f; normals.points[9].normal[2] = -0.656282f;
normals.points[10].normal[0] = -0.290381f; normals.points[10].normal[1] = -0.342020f; normals.points[10].normal[2] = -0.893701f;
normals.points[11].normal[0] = 0.145191f; normals.points[11].normal[1] = -0.342020f; normals.points[11].normal[2] = -0.916697f;
normals.points[12].normal[0] = 0.656282f; normals.points[12].normal[1] = -0.342020f; normals.points[12].normal[2] = -0.656282f;
normals.points[13].normal[0] = -0.760228f; normals.points[13].normal[1] = -0.342020f; normals.points[13].normal[2] = -0.552337f;
normals.points[14].normal[0] = 0.760228f; normals.points[14].normal[1] = -0.342020f; normals.points[14].normal[2] = -0.552337f;
normals.points[15].normal[0] = -0.893701f; normals.points[15].normal[1] = -0.342020f; normals.points[15].normal[2] = -0.290380f;
normals.points[16].normal[0] = 0.893701f; normals.points[16].normal[1] = -0.342020f; normals.points[16].normal[2] = -0.290380f;
normals.points[17].normal[0] = -0.624162f; normals.points[17].normal[1] = -0.342020f; normals.points[17].normal[2] = -0.624162f;
normals.points[18].normal[0] = 0.499329f; normals.points[18].normal[1] = -0.342020f; normals.points[18].normal[2] = -0.687268f;
normals.points[19].normal[0] = -0.893701f; normals.points[19].normal[1] = -0.342020f; normals.points[19].normal[2] = -0.290380f;
normals.points[20].normal[0] = 0.893701f; normals.points[20].normal[1] = -0.342020f; normals.points[20].normal[2] = -0.290380f;
normals.points[21].normal[0] = -0.893701f; normals.points[21].normal[1] = -0.342020f; normals.points[21].normal[2] = -0.290381f;
normals.points[22].normal[0] = 0.893701f; normals.points[22].normal[1] = -0.342020f; normals.points[22].normal[2] = -0.290381f;
normals.points[23].normal[0] = -0.939693f; normals.points[23].normal[1] = -0.342020f; normals.points[23].normal[2] = 0.000000f;
normals.points[24].normal[0] = 0.939693f; normals.points[24].normal[1] = -0.342020f; normals.points[24].normal[2] = 0.000000f;
normals.points[25].normal[0] = -0.939693f; normals.points[25].normal[1] = -0.342020f; normals.points[25].normal[2] = 0.000000f;
normals.points[26].normal[0] = 0.939693f; normals.points[26].normal[1] = -0.342020f; normals.points[26].normal[2] = 0.000000f;
normals.points[27].normal[0] = -0.939693f; normals.points[27].normal[1] = -0.342020f; normals.points[27].normal[2] = 0.000000f;
normals.points[28].normal[0] = 0.939693f; normals.points[28].normal[1] = -0.342020f; normals.points[28].normal[2] = 0.000000f;
normals.points[29].normal[0] = -0.939693f; normals.points[29].normal[1] = -0.342020f; normals.points[29].normal[2] = 0.000000f;
normals.points[30].normal[0] = 0.939693f; normals.points[30].normal[1] = -0.342020f; normals.points[30].normal[2] = 0.000000f;
// Create a shared cylinder model pointer directly
SampleConsensusModelConePtr model (new SampleConsensusModelCone<PointXYZ, Normal> (cloud.makeShared ()));
model->setInputNormals (normals.makeShared ());
// Create the RANSAC object
RandomSampleConsensus<PointXYZ> sac (model, 0.03);
// Algorithm tests
bool result = sac.computeModel ();
ASSERT_EQ (result, true);
std::vector<int> sample;
sac.getModel (sample);
EXPECT_EQ (int (sample.size ()), 3);
std::vector<int> inliers;
sac.getInliers (inliers);
EXPECT_EQ (int (inliers.size ()), 31);
Eigen::VectorXf coeff;
sac.getModelCoefficients (coeff);
EXPECT_EQ (int (coeff.size ()), 7);
EXPECT_NEAR (coeff[0], 0, 1e-2);
EXPECT_NEAR (coeff[1], 0.1, 1e-2);
EXPECT_NEAR (coeff[6], 0.349066, 1e-2);
Eigen::VectorXf coeff_refined;
model->optimizeModelCoefficients (inliers, coeff, coeff_refined);
EXPECT_EQ (int (coeff_refined.size ()), 7);
EXPECT_NEAR (coeff_refined[6], 0.349066 , 1e-2);
}
void Algorithm::playFromFile(string filename, int mode)
{
PointCloudT::Ptr cloud(new PointCloudT);
interface = new pcl::OpenNIGrabber(filename);
f = boost::bind(&cloud_cb_, _1, cloud, &new_cloud_available_flag);
interface->registerCallback(f);
interface->start();
// Wait for the first frame:
while (!new_cloud_available_flag)
boost::this_thread::sleep(boost::posix_time::milliseconds(1));
new_cloud_available_flag = false;
cloud_mutex.lock(); // for not overwriting the point cloud
// Display pointcloud:
pcl::visualization::PointCloudColorHandlerRGBField<PointT> rgb(cloud);
viewer->addPointCloud<PointT>(cloud, rgb, "input_cloud");
viewer->setCameraPosition(0, 0, -2, 0, -1, 0, 0);
// Add point picking callback to viewer:
//setFloor(viewer, svm_filename, voxel_size, rgb_intrinsics_matrix, min_height, max_height);
// Main loop:
if (mode == 1)
{
cloud_mutex.unlock();
mainLoopPlain(viewer, cloud, new_cloud_available_flag);
}
if (mode == 2)
{
callback_args cb_args;
PointCloudT::Ptr clicked_points_3d(new PointCloudT);
cb_args.clicked_points_3d = clicked_points_3d;
cb_args.viewerPtr = pcl::visualization::PCLVisualizer::Ptr(viewer);
viewer->registerPointPickingCallback(pp_callback, (void*)&cb_args);
cout << "Kliknij w 3 punkty na podlodze trzymajac wcisniety SHIFT";
viewer->addText("Kliknij w 3 punkty na podlodze trzymajac wcisniety SHIFT", 250, 300, 20, 1, 1, 1);
viewer->addText("Nastepnie nacisnij klawisz Q", 250, 250, 20, 1, 1, 1);
// Spin until 'Q' is pressed:
viewer->spin();
std::cout << "Gotowe." << std::endl;
cloud_mutex.unlock();
// Ground plane estimation:
ground_coeffs.resize(4);
std::vector<int> clicked_points_indices;
for (unsigned int i = 0; i < clicked_points_3d->points.size(); i++)
clicked_points_indices.push_back(i);
pcl::SampleConsensusModelPlane<PointT> model_plane(clicked_points_3d);
model_plane.computeModelCoefficients(clicked_points_indices, ground_coeffs);
std::cout << "Ground plane: " << ground_coeffs(0) << " " << ground_coeffs(1) << " " << ground_coeffs(2) << " " << ground_coeffs(3) << std::endl;
// Create classifier for people detection:
pcl::people::PersonClassifier<pcl::RGB> person_classifier;
person_classifier.loadSVMFromFile(svm_filename); // load trained SVM
// People detection app initialization:
// people detection object
people_detector.setVoxelSize(voxel_size); // set the voxel size
people_detector.setIntrinsics(rgb_intrinsics_matrix); // set RGB camera intrinsic parameters
people_detector.setClassifier(person_classifier); // set person classifier
// people_detector.setHeightLimits(min_height, max_height); // set person height limits
people_detector.setPersonClusterLimits(min_height, max_height, 0.1, 8.0); // set person height limits
floor_tagged = true;
// For timing:
mainLoopAlgorithm(viewer, cloud, dist, new_cloud_available_flag);
}
if (mode == 3)
{
callback_args cb_args;
PointCloudT::Ptr clicked_points_3d(new PointCloudT);
cb_args.clicked_points_3d = clicked_points_3d;
cb_args.viewerPtr = pcl::visualization::PCLVisualizer::Ptr(viewer);
viewer->registerPointPickingCallback(pp_callback, (void*)&cb_args);
cout << "Kliknij w 3 punkty na podlodze trzymajac wcisniety SHIFT";
viewer->addText("Kliknij w 3 punkty na podlodze trzymajac wcisniety SHIFT", 250, 300, 20, 1, 1, 1);
viewer->addText("Nastepnie nacisnij klawisz Q", 250, 250, 20, 1, 1, 1);
// Spin until 'Q' is pressed:
viewer->spin();
std::cout << "Gotowe." << std::endl;
cloud_mutex.unlock();
// Ground plane estimation:
ground_coeffs.resize(4);
std::vector<int> clicked_points_indices;
for (unsigned int i = 0; i < clicked_points_3d->points.size(); i++)
clicked_points_indices.push_back(i);
pcl::SampleConsensusModelPlane<PointT> model_plane(clicked_points_3d);
model_plane.computeModelCoefficients(clicked_points_indices, ground_coeffs);
std::cout << "Ground plane: " << ground_coeffs(0) << " " << ground_coeffs(1) << " " << ground_coeffs(2) << " " << ground_coeffs(3) << std::endl;
// Create classifier for people detection:
pcl::people::PersonClassifier<pcl::RGB> person_classifier;
person_classifier.loadSVMFromFile(svm_filename); // load trained SVM
// People detection app initialization:
// people detection object
people_detector.setVoxelSize(voxel_size); // set the voxel size
people_detector.setIntrinsics(rgb_intrinsics_matrix); // set RGB camera intrinsic parameters
people_detector.setClassifier(person_classifier); // set person classifier
// people_detector.setHeightLimits(min_height, max_height); // set person height limits
people_detector.setPersonClusterLimits(min_height, max_height, 0.1, 8.0); // set person height limits
floor_tagged = true;
// For timing:
startRecording("asd", "asd", viewer, cloud, dist, new_cloud_available_flag);
}
//mainLoopPlain(viewer, cloud, new_cloud_available_flag);
}
int cPCA2::computePC(std::vector<float>& x,
size_t nrow,
size_t ncol,
bool is_center,
bool is_scale,
bool is_corr)
{
_ncol = ncol;
_nrow = nrow;
_is_center = is_center;
_is_scale = is_scale;
_is_corr = is_corr;
if (x.size() != _nrow*_ncol) { return -1; }
if ((1 == _ncol) || (1 == nrow)) { return -1; }
// convert vector to Eigen 2-dimensional matrix
_xXf.resize(_nrow, _ncol);
for (size_t i = 0; i < _nrow; ++i) {
for (size_t j = 0; j < _ncol; ++j) {
_xXf(i, j) = x[j + i*_ncol];
}
}
// mean and standard deviation for each column
Eigen::VectorXf mean_vector(_ncol),
sd_vector(_ncol);
size_t zero_sd_num = 0;
float denom = static_cast<float>((_nrow > 1) ? _nrow - 1 : 1);
mean_vector = _xXf.colwise().mean();
Eigen::VectorXf curr_col;
for (size_t i = 0; i < _ncol; ++i) {
curr_col = Eigen::VectorXf::Constant(_nrow, mean_vector(i)); // mean(x) for column x
curr_col = _xXf.col(i) - curr_col; // x - mean(x)
curr_col = curr_col.array().square(); // (x-mean(x))^2
sd_vector(i) = std::sqrt((curr_col.sum())/denom);
if (0 == sd_vector(i)) {
zero_sd_num++;
}
}
// if colums with sd == 0 are too many, don't continue calculation
if (1 > _ncol-zero_sd_num) {
return -1;
}
// delete columns with sd == 0
Eigen::MatrixXf tmp(_nrow, _ncol-zero_sd_num);
Eigen::VectorXf tmp_mean_vector(_ncol-zero_sd_num);
size_t curr_col_num = 0;
for (size_t i = 0; i < _ncol; ++i) {
if (0 != sd_vector(i)) {
tmp.col(curr_col_num) = _xXf.col(i);
tmp_mean_vector(curr_col_num) = mean_vector(i);
curr_col_num++;
}
else {
_eliminated_columns.push_back(i);
}
}
_ncol -= zero_sd_num;
_xXf = tmp;
mean_vector = tmp_mean_vector;
tmp.resize(0, 0);
tmp_mean_vector.resize(0);
// shift to zero
if (true == _is_center) {
for (size_t i = 0; i < _ncol; ++i) {
_xXf.col(i) -= Eigen::VectorXf::Constant(_nrow, mean_vector(i));
}
}
// scale to unit variance
if ( (false == _is_corr) || (true == _is_scale)) {
for (size_t i = 0; i < _ncol; ++i) {
_xXf.col(i) /= std::sqrt(_xXf.col(i).array().square().sum()/denom);
}
}
#ifndef NDEBUG
std::cout << "\nScaled matrix:\n";
std::cout << _xXf << std::endl;
std::cout << "\nMean before scaling:\n" << mean_vector.transpose();
std::cout << "\nStandard deviation before scaling:\n" << sd_vector.transpose();
#endif
// when _nrow < _ncol then svd will be used
// if corr is true and _nrow > _ncol then correlation matrix will be used
// (TODO): What about covariance?
if ((_nrow < _ncol) || (false == _is_corr)) {
_method = "svd";
Eigen::JacobiSVD<Eigen::MatrixXf> svd(_xXf, Eigen::ComputeThinV);
Eigen::VectorXf eigen_singular_values = svd.singularValues();
Eigen::VectorXf tmp_vec = eigen_singular_values.array().square();
float tmp_sum = tmp_vec.sum();
size_t lim = (_nrow < _ncol)? _nrow : _ncol;
tmp_vec /= tmp_sum;
// PC's standard deviation and
// PC's proportion of variance
_kaiser = 0;
for (size_t i = 0; i < lim; ++i) {
_sd.push_back(eigen_singular_values(i)/std::sqrt(denom));
if (_sd[i] >= 1) {
_kaiser = (unsigned int) i + 1;
}
_prop_of_var.push_back(tmp_vec(i));
}
tmp_vec.resize(0);
#ifndef NDEBUG
std::cout << "\n\nStandard deviations for PCs:\n";
copy(_sd.begin(), _sd.end(),std::ostream_iterator<float>(std::cout," "));
std::cout << "\n\nKaiser criterion: PC #" << _kaiser << std::endl;
#endif
// PC's cumulative proportion
_thresh95 = 1;
_cum_prop.push_back(_prop_of_var[0]);
for (size_t i = 1; i < _prop_of_var.size(); ++i) {
_cum_prop.push_back(_cum_prop[i-1]+_prop_of_var[i]);
if (_cum_prop[i] < 0.95) {
_thresh95 = (unsigned int) i + 1;
}
}
#ifndef NDEBUG
std::cout << "\nCumulative proportion:\n";
copy(_cum_prop.begin(), _cum_prop.end(),std::ostream_iterator<float>(std::cout," "));
std::cout << "\n\nThresh95 criterion: PC #" << _thresh95 << std::endl;
#endif
// scores
Eigen::MatrixXf eigen_scores = _xXf * svd.matrixV();
#ifndef NDEBUG
std::cout << "\n\nRotated values (scores):\n" << eigen_scores;
#endif
_scores.reserve(lim*lim);
for (size_t i = 0; i < lim; ++i) {
for (size_t j = 0; j < lim; ++j) {
_scores.push_back(eigen_scores(i, j));
}
}
eigen_scores.resize(0, 0);
#ifndef NDEBUG
std::cout << "\n\nScores in vector:\n";
copy(_scores.begin(), _scores.end(),std::ostream_iterator<float>(std::cout," "));
std::cout << "\n";
#endif
}
else { // COR OR COV MATRICES ARE HERE
_method = "cor";
// calculate covariance matrix
Eigen::MatrixXf eigen_cov; // = MatrixXf::Zero(_ncol, _ncol);
Eigen::VectorXf sds;
// (TODO) should be weighted cov matrix, even if is_center == false
eigen_cov = (1.0f /((float) _nrow/*-1*/)) * _xXf.transpose() * _xXf;
sds = eigen_cov.diagonal().array().sqrt();
Eigen::MatrixXf outer_sds = sds * sds.transpose();
eigen_cov = eigen_cov.array() / outer_sds.array();
outer_sds.resize(0, 0);
// ?if data matrix is scaled, covariance matrix is equal to correlation matrix
Eigen::EigenSolver<Eigen::MatrixXf> edc(eigen_cov);
Eigen::VectorXf eigen_eigenvalues = edc.eigenvalues().real();
Eigen::MatrixXf eigen_eigenvectors = edc.eigenvectors().real();
#ifndef NDEBUG
std::cout << eigen_cov << std::endl;
std::cout << std::endl << eigen_eigenvalues.transpose() << std::endl;
std::cout << std::endl << eigen_eigenvectors << std::endl;
#endif
// the eigenvalues and eigenvectors are not sorted
// so, we should sort them
typedef std::pair<float,int> eigen_pair;
std::vector<eigen_pair> ep;
for (size_t i = 0 ; i < _ncol; ++i) {
ep.push_back(std::make_pair(eigen_eigenvalues(i), i));
}
sort(ep.begin(), ep.end()); // ascending order by default
// sort them all in descending order
Eigen::MatrixXf eigen_eigenvectors_sorted = Eigen::MatrixXf::Zero(eigen_eigenvectors.rows(), eigen_eigenvectors.cols());
Eigen::VectorXf eigen_eigenvalues_sorted = Eigen::VectorXf::Zero(_ncol);
int colnum = 0;
for (int i = (int) ep.size()-1; i > -1; i--) {
eigen_eigenvalues_sorted(colnum) = ep[i].first;
eigen_eigenvectors_sorted.col(colnum++) += eigen_eigenvectors.col(ep[i].second);
}
#ifndef NDEBUG
std::cout << std::endl << eigen_eigenvalues_sorted.transpose() << std::endl;
std::cout << std::endl << eigen_eigenvectors_sorted << std::endl;
#endif
// we don't need not sorted arrays anymore
eigen_eigenvalues.resize(0);
eigen_eigenvectors.resize(0, 0);
_sd.clear();
_prop_of_var.clear();
_kaiser = 0;
float tmp_sum = eigen_eigenvalues_sorted.sum();
for (size_t i = 0; i < _ncol; ++i) {
_sd.push_back(std::sqrt(eigen_eigenvalues_sorted(i)));
if (_sd[i] >= 1) {
_kaiser = (unsigned int) i + 1;
}
_prop_of_var.push_back(eigen_eigenvalues_sorted(i)/tmp_sum);
}
#ifndef NDEBUG
std::cout << "\nStandard deviations for PCs:\n";
copy(_sd.begin(), _sd.end(), std::ostream_iterator<float>(std::cout," "));
std::cout << "\nProportion of variance:\n";
copy(_prop_of_var.begin(), _prop_of_var.end(), std::ostream_iterator<float>(std::cout," "));
std::cout << "\nKaiser criterion: PC #" << _kaiser << std::endl;
#endif
// PC's cumulative proportion
_cum_prop.clear();
_thresh95 = 1;
_cum_prop.push_back(_prop_of_var[0]);
for (size_t i = 1; i < _prop_of_var.size(); ++i) {
_cum_prop.push_back(_cum_prop[i-1]+_prop_of_var[i]);
if (_cum_prop[i] < 0.95) {
_thresh95 = (unsigned int) i + 1;
}
}
#ifndef NDEBUG
std::cout << "\n\nCumulative proportions:\n";
copy(_cum_prop.begin(), _cum_prop.end(), std::ostream_iterator<float>(std::cout," "));
std::cout << "\n\n95% threshold: PC #" << _thresh95 << std::endl;
#endif
// scores for PCA with correlation matrix
// scale before calculating new values
for (size_t i = 0; i < _ncol; ++i) {
_xXf.col(i) /= sds(i);
}
sds.resize(0);
Eigen::MatrixXf eigen_scores = _xXf * eigen_eigenvectors_sorted;
#ifndef NDEBUG
std::cout << "\n\nRotated values (scores):\n" << eigen_scores;
#endif
_scores.clear();
_scores.reserve(_ncol*_nrow);
for (size_t i = 0; i < _nrow; ++i) {
for (size_t j = 0; j < _ncol; ++j) {
_scores.push_back(eigen_scores(i, j));
}
}
eigen_scores.resize(0, 0);
#ifndef NDEBUG
std::cout << "\n\nScores in vector:\n";
copy(_scores.begin(), _scores.end(), std::ostream_iterator<float>(std::cout," "));
std::cout << "\n";
#endif
}
return 0;
}
int main(int argc, char *argv[])
{
if ( argc != 3 )
{
std::cout<<"usage: "<< argv[0] <<" <input file> <output file>\n";
return 1;
}
std::ifstream infile(argv[1]);
std::ofstream outfile(argv[2]);
float poissonRatio, youngModulus;
infile >> poissonRatio >> youngModulus;
Eigen::Matrix3f D;
D <<
1.0f, poissonRatio, 0.0f,
poissonRatio, 1.0, 0.0f,
0.0f, 0.0f, (1.0f - poissonRatio) / 2.0f;
D *= youngModulus / (1.0f - pow(poissonRatio, 2.0f));
infile >> nodesCount;
nodesX.resize(nodesCount);
nodesY.resize(nodesCount);
for (int i = 0; i < nodesCount; ++i)
{
infile >> nodesX[i] >> nodesY[i];
}
int elementCount;
infile >> elementCount;
for (int i = 0; i < elementCount; ++i)
{
Element element;
infile >> element.nodesIds[0] >> element.nodesIds[1] >> element.nodesIds[2];
elements.push_back(element);
}
int constraintCount;
infile >> constraintCount;
for (int i = 0; i < constraintCount; ++i)
{
Constraint constraint;
int type;
infile >> constraint.node >> type;
constraint.type = static_cast<Constraint::Type>(type);
constraints.push_back(constraint);
}
loads.resize(2 * nodesCount);
loads.setZero();
int loadsCount;
infile >> loadsCount;
for (int i = 0; i < loadsCount; ++i)
{
int node;
float x, y;
infile >> node >> x >> y;
loads[2 * node + 0] = x;
loads[2 * node + 1] = y;
}
std::vector<Eigen::Triplet<float> > triplets;
for (std::vector<Element>::iterator it = elements.begin(); it != elements.end(); ++it)
{
it->CalculateStiffnessMatrix(D, triplets);
}
Eigen::SparseMatrix<float> globalK(2 * nodesCount, 2 * nodesCount);
globalK.setFromTriplets(triplets.begin(), triplets.end());
ApplyConstraints(globalK, constraints);
Eigen::SimplicialLDLT<Eigen::SparseMatrix<float> > solver(globalK);
Eigen::VectorXf displacements = solver.solve(loads);
outfile << displacements << std::endl;
for (std::vector<Element>::iterator it = elements.begin(); it != elements.end(); ++it)
{
Eigen::Matrix<float, 6, 1> delta;
delta << displacements.segment<2>(2 * it->nodesIds[0]),
displacements.segment<2>(2 * it->nodesIds[1]),
displacements.segment<2>(2 * it->nodesIds[2]);
Eigen::Vector3f sigma = D * it->B * delta;
float sigma_mises = sqrt(sigma[0] * sigma[0] - sigma[0] * sigma[1] + sigma[1] * sigma[1] + 3.0f * sigma[2] * sigma[2]);
outfile << sigma_mises << std::endl;
}
return 0;
}
template <typename PointT> void
pcl::SampleConsensusModelPlane<PointT>::projectPoints (
const std::vector<int> &inliers, const Eigen::VectorXf &model_coefficients, PointCloud &projected_points, bool copy_data_fields)
{
// Needs a valid set of model coefficients
if (model_coefficients.size () != 4)
{
PCL_ERROR ("[pcl::SampleConsensusModelPlane::projectPoints] Invalid number of model coefficients given (%zu)!\n", model_coefficients.size ());
return;
}
projected_points.header = input_->header;
projected_points.is_dense = input_->is_dense;
Eigen::Vector4f mc (model_coefficients[0], model_coefficients[1], model_coefficients[2], 0);
// normalize the vector perpendicular to the plane...
mc.normalize ();
// ... and store the resulting normal as a local copy of the model coefficients
Eigen::Vector4f tmp_mc = model_coefficients;
tmp_mc[0] = mc[0];
tmp_mc[1] = mc[1];
tmp_mc[2] = mc[2];
// Copy all the data fields from the input cloud to the projected one?
if (copy_data_fields)
{
// Allocate enough space and copy the basics
projected_points.points.resize (input_->points.size ());
projected_points.width = input_->width;
projected_points.height = input_->height;
typedef typename pcl::traits::fieldList<PointT>::type FieldList;
// Iterate over each point
for (size_t i = 0; i < input_->points.size (); ++i)
// Iterate over each dimension
pcl::for_each_type <FieldList> (NdConcatenateFunctor <PointT, PointT> (input_->points[i], projected_points.points[i]));
// Iterate through the 3d points and calculate the distances from them to the plane
for (size_t i = 0; i < inliers.size (); ++i)
{
// Calculate the distance from the point to the plane
Eigen::Vector4f p (input_->points[inliers[i]].x,
input_->points[inliers[i]].y,
input_->points[inliers[i]].z,
1);
// use normalized coefficients to calculate the scalar projection
float distance_to_plane = tmp_mc.dot (p);
pcl::Vector4fMap pp = projected_points.points[inliers[i]].getVector4fMap ();
pp = p - mc * distance_to_plane; // mc[3] = 0, therefore the 3rd coordinate is safe
}
}
else
{
// Allocate enough space and copy the basics
projected_points.points.resize (inliers.size ());
projected_points.width = static_cast<uint32_t> (inliers.size ());
projected_points.height = 1;
typedef typename pcl::traits::fieldList<PointT>::type FieldList;
// Iterate over each point
for (size_t i = 0; i < inliers.size (); ++i)
// Iterate over each dimension
pcl::for_each_type <FieldList> (NdConcatenateFunctor <PointT, PointT> (input_->points[inliers[i]], projected_points.points[i]));
// Iterate through the 3d points and calculate the distances from them to the plane
for (size_t i = 0; i < inliers.size (); ++i)
{
// Calculate the distance from the point to the plane
Eigen::Vector4f p (input_->points[inliers[i]].x,
input_->points[inliers[i]].y,
input_->points[inliers[i]].z,
1);
// use normalized coefficients to calculate the scalar projection
float distance_to_plane = tmp_mc.dot (p);
pcl::Vector4fMap pp = projected_points.points[i].getVector4fMap ();
pp = p - mc * distance_to_plane; // mc[3] = 0, therefore the 3rd coordinate is safe
}
}
}
template <typename PointT> bool
pcl::SampleConsensusModelSphere<PointT>::computeModelCoefficients (
const std::vector<int> &samples, Eigen::VectorXf &model_coefficients)
{
// Need 4 samples
if (samples.size () != 4)
{
PCL_ERROR ("[pcl::SampleConsensusModelSphere::computeModelCoefficients] Invalid set of samples given (%zu)!\n", samples.size ());
return (false);
}
Eigen::Matrix4f temp;
for (int i = 0; i < 4; i++)
{
temp (i, 0) = input_->points[samples[i]].x;
temp (i, 1) = input_->points[samples[i]].y;
temp (i, 2) = input_->points[samples[i]].z;
temp (i, 3) = 1;
}
float m11 = temp.determinant ();
if (m11 == 0)
return (false); // the points don't define a sphere!
for (int i = 0; i < 4; ++i)
temp (i, 0) = (input_->points[samples[i]].x) * (input_->points[samples[i]].x) +
(input_->points[samples[i]].y) * (input_->points[samples[i]].y) +
(input_->points[samples[i]].z) * (input_->points[samples[i]].z);
float m12 = temp.determinant ();
for (int i = 0; i < 4; ++i)
{
temp (i, 1) = temp (i, 0);
temp (i, 0) = input_->points[samples[i]].x;
}
float m13 = temp.determinant ();
for (int i = 0; i < 4; ++i)
{
temp (i, 2) = temp (i, 1);
temp (i, 1) = input_->points[samples[i]].y;
}
float m14 = temp.determinant ();
for (int i = 0; i < 4; ++i)
{
temp (i, 0) = temp (i, 2);
temp (i, 1) = input_->points[samples[i]].x;
temp (i, 2) = input_->points[samples[i]].y;
temp (i, 3) = input_->points[samples[i]].z;
}
float m15 = temp.determinant ();
// Center (x , y, z)
model_coefficients.resize (4);
model_coefficients[0] = 0.5f * m12 / m11;
model_coefficients[1] = 0.5f * m13 / m11;
model_coefficients[2] = 0.5f * m14 / m11;
// Radius
model_coefficients[3] = sqrtf (
model_coefficients[0] * model_coefficients[0] +
model_coefficients[1] * model_coefficients[1] +
model_coefficients[2] * model_coefficients[2] - m15 / m11);
return (true);
}
template <typename PointT> void
pcl::SampleConsensusModelCircle2D<PointT>::projectPoints (
const std::vector<int> &inliers, const Eigen::VectorXf &model_coefficients,
PointCloud &projected_points, bool copy_data_fields)
{
// Needs a valid set of model coefficients
if (model_coefficients.size () != 3)
{
PCL_ERROR ("[pcl::SampleConsensusModelCircle2D::projectPoints] Invalid number of model coefficients given (%zu)!\n", model_coefficients.size ());
return;
}
projected_points.header = input_->header;
projected_points.is_dense = input_->is_dense;
// Copy all the data fields from the input cloud to the projected one?
if (copy_data_fields)
{
// Allocate enough space and copy the basics
projected_points.points.resize (input_->points.size ());
projected_points.width = input_->width;
projected_points.height = input_->height;
typedef typename pcl::traits::fieldList<PointT>::type FieldList;
// Iterate over each point
for (size_t i = 0; i < projected_points.points.size (); ++i)
// Iterate over each dimension
pcl::for_each_type <FieldList> (NdConcatenateFunctor <PointT, PointT> (input_->points[i], projected_points.points[i]));
// Iterate through the 3d points and calculate the distances from them to the plane
for (size_t i = 0; i < inliers.size (); ++i)
{
float dx = input_->points[inliers[i]].x - model_coefficients[0];
float dy = input_->points[inliers[i]].y - model_coefficients[1];
float a = sqrtf ( (model_coefficients[2] * model_coefficients[2]) / (dx * dx + dy * dy) );
projected_points.points[inliers[i]].x = a * dx + model_coefficients[0];
projected_points.points[inliers[i]].y = a * dy + model_coefficients[1];
}
}
else
{
// Allocate enough space and copy the basics
projected_points.points.resize (inliers.size ());
projected_points.width = static_cast<uint32_t> (inliers.size ());
projected_points.height = 1;
typedef typename pcl::traits::fieldList<PointT>::type FieldList;
// Iterate over each point
for (size_t i = 0; i < inliers.size (); ++i)
// Iterate over each dimension
pcl::for_each_type <FieldList> (NdConcatenateFunctor <PointT, PointT> (input_->points[inliers[i]], projected_points.points[i]));
// Iterate through the 3d points and calculate the distances from them to the plane
for (size_t i = 0; i < inliers.size (); ++i)
{
float dx = input_->points[inliers[i]].x - model_coefficients[0];
float dy = input_->points[inliers[i]].y - model_coefficients[1];
float a = sqrtf ( (model_coefficients[2] * model_coefficients[2]) / (dx * dx + dy * dy) );
projected_points.points[i].x = a * dx + model_coefficients[0];
projected_points.points[i].y = a * dy + model_coefficients[1];
}
}
}
template<typename PointT> inline void
pcl::PCA<PointT>::update (const PointT& input_point, FLAG flag)
{
if (!compute_done_)
initCompute ();
if (!compute_done_)
PCL_THROW_EXCEPTION (InitFailedException, "[pcl::PCA::update] PCA initCompute failed");
Eigen::Vector3f input (input_point.x, input_point.y, input_point.z);
const size_t n = eigenvectors_.cols ();// number of eigen vectors
Eigen::VectorXf meanp = (float(n) * (mean_.head<3>() + input)) / float(n + 1);
Eigen::VectorXf a = eigenvectors_.transpose() * (input - mean_.head<3>());
Eigen::VectorXf y = (eigenvectors_ * a) + mean_.head<3>();
Eigen::VectorXf h = y - input;
if (h.norm() > 0)
h.normalize ();
else
h.setZero ();
float gamma = h.dot(input - mean_.head<3>());
Eigen::MatrixXf D = Eigen::MatrixXf::Zero (a.size() + 1, a.size() + 1);
D.block(0,0,n,n) = a * a.transpose();
D /= float(n)/float((n+1) * (n+1));
for(std::size_t i=0; i < a.size(); i++) {
D(i,i)+= float(n)/float(n+1)*eigenvalues_(i);
D(D.rows()-1,i) = float(n) / float((n+1) * (n+1)) * gamma * a(i);
D(i,D.cols()-1) = D(D.rows()-1,i);
D(D.rows()-1,D.cols()-1) = float(n)/float((n+1) * (n+1)) * gamma * gamma;
}
Eigen::MatrixXf R(D.rows(), D.cols());
Eigen::EigenSolver<Eigen::MatrixXf> D_evd (D, false);
Eigen::VectorXf alphap = D_evd.eigenvalues().real();
eigenvalues_.resize(eigenvalues_.size() +1);
for(std::size_t i=0;i<eigenvalues_.size();i++) {
eigenvalues_(i) = alphap(eigenvalues_.size()-i-1);
R.col(i) = D.col(D.cols()-i-1);
}
Eigen::MatrixXf Up = Eigen::MatrixXf::Zero(eigenvectors_.rows(), eigenvectors_.cols()+1);
Up.topLeftCorner(eigenvectors_.rows(),eigenvectors_.cols()) = eigenvectors_;
Up.rightCols<1>() = h;
eigenvectors_ = Up*R;
if (!basis_only_) {
Eigen::Vector3f etha = Up.transpose() * (mean_.head<3>() - meanp);
coefficients_.resize(coefficients_.rows()+1,coefficients_.cols()+1);
for(std::size_t i=0; i < (coefficients_.cols() - 1); i++) {
coefficients_(coefficients_.rows()-1,i) = 0;
coefficients_.col(i) = (R.transpose() * coefficients_.col(i)) + etha;
}
a.resize(a.size()+1);
a(a.size()-1) = 0;
coefficients_.col(coefficients_.cols()-1) = (R.transpose() * a) + etha;
}
mean_.head<3>() = meanp;
switch (flag)
{
case increase:
if (eigenvectors_.rows() >= eigenvectors_.cols())
break;
case preserve:
if (!basis_only_)
coefficients_ = coefficients_.topRows(coefficients_.rows() - 1);
eigenvectors_ = eigenvectors_.leftCols(eigenvectors_.cols() - 1);
eigenvalues_.resize(eigenvalues_.size()-1);
break;
default:
PCL_ERROR("[pcl::PCA] unknown flag\n");
}
}
int Fit(Vector& res_G, // residual under NULL -- may change when permuting
Vector& v_G, // variance under NULL -- may change when permuting
Matrix& X_G, // covariance
Matrix& G_G, // genotype
Vector& w_G) // weight
{
this->nPeople = X_G.rows;
this->nMarker = G_G.cols;
this->nCovariate = X_G.cols;
// calculation w_sqrt
G_to_Eigen(w_G, &this->w_sqrt);
w_sqrt = w_sqrt.cwiseSqrt();
// calculate K = G * W * G'
Eigen::MatrixXf G;
G_to_Eigen(G_G, &G);
this->K_sqrt.noalias() = w_sqrt.asDiagonal() * G.transpose();
// calculate Q = ||res * K||
Eigen::VectorXf res;
G_to_Eigen(res_G, &res);
this->Q = (this->K_sqrt * res).squaredNorm();
// calculate P0 = V - V X (X' V X)^(-1) X' V
Eigen::VectorXf v;
G_to_Eigen(v_G, &v);
if (this->nCovariate == 1) {
P0 = -v * v.transpose() / v.sum();
// printf("dim(P0) = %d, %d\n", P0.rows(), P0.cols());
// printf("dim(v) = %d\n", v.size());
P0.diagonal() += v;
// printf("dim(v) = %d\n", v.size());
} else {
Eigen::MatrixXf X;
G_to_Eigen(X_G, &X);
Eigen::MatrixXf XtV; // X^t V
XtV.noalias() = X.transpose() * v.asDiagonal();
P0 = -XtV.transpose() * ((XtV * X).inverse()) * XtV;
P0.diagonal() += v;
}
// dump();
// Eigen::MatrixXf tmp = K_sqrt * P0 * K_sqrt.transpose();
// dumpToFile(tmp, "out.tmp");
// eigen decomposition
Eigen::SelfAdjointEigenSolver<Eigen::MatrixXf> es;
es.compute(K_sqrt * P0 * K_sqrt.transpose());
#ifdef DEBUG
std::ofstream k("K");
k << K_sqrt;
k.close();
#endif
// std::ofstream p("P0");
// p << P0;
// p.close();
this->mixChiSq.reset();
int r_ub = std::min(nPeople, nMarker);
int r = 0; // es.eigenvalues().size();
int eigen_len = es.eigenvalues().size();
for (int i = eigen_len - 1; i >= 0; i--) {
if (es.eigenvalues()[i] > ZBOUND && r < r_ub) {
this->mixChiSq.addLambda(es.eigenvalues()[i]);
r++;
} else {
break;
}
}
// calculate p-value
this->pValue = this->mixChiSq.getPvalue(this->Q);
if (this->pValue <= 0.0 || this->pValue == 1.0) {
this->pValue = this->mixChiSq.getLiuPvalue(this->Q);
}
return 0;
};
template <typename PointInT, typename PointNT, typename PointOutT> void
pcl::PFHRGBEstimation<PointInT, PointNT, PointOutT>::computePointPFHRGBSignature (
const pcl::PointCloud<PointInT> &cloud, const pcl::PointCloud<PointNT> &normals,
const std::vector<int> &indices, int nr_split, Eigen::VectorXf &pfhrgb_histogram)
{
int h_index, h_p;
// Clear the resultant point histogram
pfhrgb_histogram.setZero ();
// Factorization constant
float hist_incr = 100.0f / static_cast<float> (indices.size () * indices.size () - 1);
// Iterate over all the points in the neighborhood
for (size_t i_idx = 0; i_idx < indices.size (); ++i_idx)
{
for (size_t j_idx = 0; j_idx < indices.size (); ++j_idx)
{
// Avoid unnecessary returns
if (i_idx == j_idx)
continue;
// Compute the pair NNi to NNj
if (!computeRGBPairFeatures (cloud, normals, indices[i_idx], indices[j_idx],
pfhrgb_tuple_[0], pfhrgb_tuple_[1], pfhrgb_tuple_[2], pfhrgb_tuple_[3],
pfhrgb_tuple_[4], pfhrgb_tuple_[5], pfhrgb_tuple_[6]))
continue;
// Normalize the f1, f2, f3, f5, f6, f7 features and push them in the histogram
f_index_[0] = static_cast<int> (floor (nr_split * ((pfhrgb_tuple_[0] + M_PI) * d_pi_)));
if (f_index_[0] < 0) f_index_[0] = 0;
if (f_index_[0] >= nr_split) f_index_[0] = nr_split - 1;
f_index_[1] = static_cast<int> (floor (nr_split * ((pfhrgb_tuple_[1] + 1.0) * 0.5)));
if (f_index_[1] < 0) f_index_[1] = 0;
if (f_index_[1] >= nr_split) f_index_[1] = nr_split - 1;
f_index_[2] = static_cast<int> (floor (nr_split * ((pfhrgb_tuple_[2] + 1.0) * 0.5)));
if (f_index_[2] < 0) f_index_[2] = 0;
if (f_index_[2] >= nr_split) f_index_[2] = nr_split - 1;
// color ratios are in [-1, 1]
f_index_[4] = static_cast<int> (floor (nr_split * ((pfhrgb_tuple_[4] + 1.0) * 0.5)));
if (f_index_[4] < 0) f_index_[4] = 0;
if (f_index_[4] >= nr_split) f_index_[4] = nr_split - 1;
f_index_[5] = static_cast<int> (floor (nr_split * ((pfhrgb_tuple_[5] + 1.0) * 0.5)));
if (f_index_[5] < 0) f_index_[5] = 0;
if (f_index_[5] >= nr_split) f_index_[5] = nr_split - 1;
f_index_[6] = static_cast<int> (floor (nr_split * ((pfhrgb_tuple_[6] + 1.0) * 0.5)));
if (f_index_[6] < 0) f_index_[6] = 0;
if (f_index_[6] >= nr_split) f_index_[6] = nr_split - 1;
// Copy into the histogram
h_index = 0;
h_p = 1;
for (int d = 0; d < 3; ++d)
{
h_index += h_p * f_index_[d];
h_p *= nr_split;
}
pfhrgb_histogram[h_index] += hist_incr;
// and the colors
h_index = 125;
h_p = 1;
for (int d = 4; d < 7; ++d)
{
h_index += h_p * f_index_[d];
h_p *= nr_split;
}
pfhrgb_histogram[h_index] += hist_incr;
}
}
}
template <typename PointT> void
pcl::SampleConsensusModelCircle3D<PointT>::projectPoints (
const std::vector<int> &inliers, const Eigen::VectorXf &model_coefficients,
PointCloud &projected_points, bool copy_data_fields)
{
// Needs a valid set of model coefficients
if (model_coefficients.size () != 7)
{
PCL_ERROR ("[pcl::SampleConsensusModelCircle3D::projectPoints] Invalid number of model coefficients given (%lu)!\n", model_coefficients.size ());
return;
}
projected_points.header = input_->header;
projected_points.is_dense = input_->is_dense;
// Copy all the data fields from the input cloud to the projected one?
if (copy_data_fields)
{
// Allocate enough space and copy the basics
projected_points.points.resize (input_->points.size ());
projected_points.width = input_->width;
projected_points.height = input_->height;
typedef typename pcl::traits::fieldList<PointT>::type FieldList;
// Iterate over each point
for (size_t i = 0; i < projected_points.points.size (); ++i)
// Iterate over each dimension
pcl::for_each_type <FieldList> (NdConcatenateFunctor <PointT, PointT> (input_->points[i], projected_points.points[i]));
// Iterate through the 3d points and calculate the distances from them to the plane
for (size_t i = 0; i < inliers.size (); ++i)
{
// what i have:
// P : Sample Point
Eigen::Vector3d P (input_->points[inliers[i]].x, input_->points[inliers[i]].y, input_->points[inliers[i]].z);
// C : Circle Center
Eigen::Vector3d C (model_coefficients[0], model_coefficients[1], model_coefficients[2]);
// N : Circle (Plane) Normal
Eigen::Vector3d N (model_coefficients[4], model_coefficients[5], model_coefficients[6]);
// r : Radius
double r = model_coefficients[3];
Eigen::Vector3d helper_vectorPC = P - C;
// 1.1. get line parameter
//float lambda = (helper_vectorPC.dot(N)) / N.squaredNorm() ;
double lambda = (-(helper_vectorPC.dot (N))) / N.dot (N);
// Projected Point on plane
Eigen::Vector3d P_proj = P + lambda * N;
Eigen::Vector3d helper_vectorP_projC = P_proj - C;
// K : Point on Circle
Eigen::Vector3d K = C + r * helper_vectorP_projC.normalized ();
projected_points.points[i].x = static_cast<float> (K[0]);
projected_points.points[i].y = static_cast<float> (K[1]);
projected_points.points[i].z = static_cast<float> (K[2]);
}
}
else
{
// Allocate enough space and copy the basics
projected_points.points.resize (inliers.size ());
projected_points.width = uint32_t (inliers.size ());
projected_points.height = 1;
typedef typename pcl::traits::fieldList<PointT>::type FieldList;
// Iterate over each point
for (size_t i = 0; i < inliers.size (); ++i)
// Iterate over each dimension
pcl::for_each_type <FieldList> (NdConcatenateFunctor <PointT, PointT> (input_->points[inliers[i]], projected_points.points[i]));
// Iterate through the 3d points and calculate the distances from them to the plane
for (size_t i = 0; i < inliers.size (); ++i)
{
// what i have:
// P : Sample Point
Eigen::Vector3d P (input_->points[inliers[i]].x, input_->points[inliers[i]].y, input_->points[inliers[i]].z);
// C : Circle Center
Eigen::Vector3d C (model_coefficients[0], model_coefficients[1], model_coefficients[2]);
// N : Circle (Plane) Normal
Eigen::Vector3d N (model_coefficients[4], model_coefficients[5], model_coefficients[6]);
// r : Radius
double r = model_coefficients[3];
Eigen::Vector3d helper_vectorPC = P - C;
// 1.1. get line parameter
double lambda = (-(helper_vectorPC.dot (N))) / N.dot (N);
// Projected Point on plane
Eigen::Vector3d P_proj = P + lambda * N;
Eigen::Vector3d helper_vectorP_projC = P_proj - C;
// K : Point on Circle
Eigen::Vector3d K = C + r * helper_vectorP_projC.normalized ();
projected_points.points[i].x = static_cast<float> (K[0]);
projected_points.points[i].y = static_cast<float> (K[1]);
projected_points.points[i].z = static_cast<float> (K[2]);
}
}
}
bool key_down(igl::viewer::Viewer& viewer, unsigned char key, int modifier)
{
if (key == '1')
{
display_mode = 1;
update_display(viewer);
}
if (key == '2')
{
display_mode = 2;
update_display(viewer);
}
if (key == '3')
{
display_mode = 3;
update_display(viewer);
}
if (key == '4')
{
display_mode = 4;
update_display(viewer);
}
if (key == '5')
{
display_mode = 5;
update_display(viewer);
}
if (key == '6')
{
display_mode = 6;
update_display(viewer);
}
if (key == '7')
{
display_mode = 7;
update_display(viewer);
}
if (key == '8')
{
display_mode = 8;
update_display(viewer);
}
if (key == '9')
{
display_mode = 9;
update_display(viewer);
}
if (key == '0')
{
display_mode = 0;
update_display(viewer);
}
if (key == 'A')
{
//do a batch of iterations
printf("--Improving Curl--\n");
for (int bi = 0; bi<5; ++bi)
{
printf("\n\n **** Batch %d ****\n", iter);
igl::integrable_polyvector_fields_solve(ipfdata, params, two_pv, iter ==0);
iter++;
params.wSmooth *= params.redFactor_wsmooth;
}
// Post process current field
// Compute curl_minimizing matchings and curl
printf("--Matchings and curl--\n");
Eigen::MatrixXi match_ab, match_ba; // matchings across interior edges
double avgCurl = igl::polyvector_field_matchings(two_pv, V, F, true, match_ab, match_ba, curl);
double maxCurl = curl.maxCoeff();
printf("curl -- max: %.5g, avg: %.5g\n", maxCurl, avgCurl);
// Compute singularities
printf("--Singularities--\n");
igl::polyvector_field_singularities_from_matchings(V, F, match_ab, match_ba, singularities);
printf("#singularities: %ld\n", singularities.rows());
// Get mesh cuts based on singularities
printf("--Cuts--\n");
igl::polyvector_field_cut_mesh_with_singularities(V, F, singularities, cuts);
// Comb field
printf("--Combing--\n");
Eigen::MatrixXd combed;
igl::polyvector_field_comb_from_matchings_and_cuts(V, F, two_pv, match_ab, match_ba, cuts, combed);
// Reconstruct integrable vector fields from combed field
printf("--Cut mesh--\n");
igl::cut_mesh(V, F, cuts, Vcut, Fcut);
printf("--Poisson--\n");
double avgPoisson = igl::polyvector_field_poisson_reconstruction(Vcut, Fcut, combed, scalars, two_pv_poisson, poisson_error);
double maxPoisson = poisson_error.maxCoeff();
printf("poisson error -- max: %.5g, avg: %.5g\n", maxPoisson, avgPoisson);
update_display(viewer);
}
return false;
}
TEST (RANSAC, SampleConsensusModelNormalSphere)
{
srand (0);
// Use a custom point cloud for these tests until we need something better
PointCloud<PointXYZ> cloud;
PointCloud<Normal> normals;
cloud.points.resize (27); normals.points.resize (27);
cloud.points[0].x = -0.014695f; cloud.points[0].y = 0.009549f; cloud.points[0].z = 0.954775f;
cloud.points[1].x = 0.014695f; cloud.points[1].y = 0.009549f; cloud.points[1].z = 0.954775f;
cloud.points[2].x = -0.014695f; cloud.points[2].y = 0.040451f; cloud.points[2].z = 0.954775f;
cloud.points[3].x = 0.014695f; cloud.points[3].y = 0.040451f; cloud.points[3].z = 0.954775f;
cloud.points[4].x = -0.009082f; cloud.points[4].y = -0.015451f; cloud.points[4].z = 0.972049f;
cloud.points[5].x = 0.009082f; cloud.points[5].y = -0.015451f; cloud.points[5].z = 0.972049f;
cloud.points[6].x = -0.038471f; cloud.points[6].y = 0.009549f; cloud.points[6].z = 0.972049f;
cloud.points[7].x = 0.038471f; cloud.points[7].y = 0.009549f; cloud.points[7].z = 0.972049f;
cloud.points[8].x = -0.038471f; cloud.points[8].y = 0.040451f; cloud.points[8].z = 0.972049f;
cloud.points[9].x = 0.038471f; cloud.points[9].y = 0.040451f; cloud.points[9].z = 0.972049f;
cloud.points[10].x = -0.009082f; cloud.points[10].y = 0.065451f; cloud.points[10].z = 0.972049f;
cloud.points[11].x = 0.009082f; cloud.points[11].y = 0.065451f; cloud.points[11].z = 0.972049f;
cloud.points[12].x = -0.023776f; cloud.points[12].y = -0.015451f; cloud.points[12].z = 0.982725f;
cloud.points[13].x = 0.023776f; cloud.points[13].y = -0.015451f; cloud.points[13].z = 0.982725f;
cloud.points[14].x = -0.023776f; cloud.points[14].y = 0.065451f; cloud.points[14].z = 0.982725f;
cloud.points[15].x = 0.023776f; cloud.points[15].y = 0.065451f; cloud.points[15].z = 0.982725f;
cloud.points[16].x = -0.000000f; cloud.points[16].y = -0.025000f; cloud.points[16].z = 1.000000f;
cloud.points[17].x = 0.000000f; cloud.points[17].y = -0.025000f; cloud.points[17].z = 1.000000f;
cloud.points[18].x = -0.029389f; cloud.points[18].y = -0.015451f; cloud.points[18].z = 1.000000f;
cloud.points[19].x = 0.029389f; cloud.points[19].y = -0.015451f; cloud.points[19].z = 1.000000f;
cloud.points[20].x = -0.047553f; cloud.points[20].y = 0.009549f; cloud.points[20].z = 1.000000f;
cloud.points[21].x = 0.047553f; cloud.points[21].y = 0.009549f; cloud.points[21].z = 1.000000f;
cloud.points[22].x = -0.047553f; cloud.points[22].y = 0.040451f; cloud.points[22].z = 1.000000f;
cloud.points[23].x = 0.047553f; cloud.points[23].y = 0.040451f; cloud.points[23].z = 1.000000f;
cloud.points[24].x = -0.029389f; cloud.points[24].y = 0.065451f; cloud.points[24].z = 1.000000f;
cloud.points[25].x = 0.029389f; cloud.points[25].y = 0.065451f; cloud.points[25].z = 1.000000f;
cloud.points[26].x = 0.000000f; cloud.points[26].y = 0.075000f; cloud.points[26].z = 1.000000f;
normals.points[0].normal[0] = -0.293893f; normals.points[0].normal[1] = -0.309017f; normals.points[0].normal[2] = -0.904509f;
normals.points[1].normal[0] = 0.293893f; normals.points[1].normal[1] = -0.309017f; normals.points[1].normal[2] = -0.904508f;
normals.points[2].normal[0] = -0.293893f; normals.points[2].normal[1] = 0.309017f; normals.points[2].normal[2] = -0.904509f;
normals.points[3].normal[0] = 0.293893f; normals.points[3].normal[1] = 0.309017f; normals.points[3].normal[2] = -0.904508f;
normals.points[4].normal[0] = -0.181636f; normals.points[4].normal[1] = -0.809017f; normals.points[4].normal[2] = -0.559017f;
normals.points[5].normal[0] = 0.181636f; normals.points[5].normal[1] = -0.809017f; normals.points[5].normal[2] = -0.559017f;
normals.points[6].normal[0] = -0.769421f; normals.points[6].normal[1] = -0.309017f; normals.points[6].normal[2] = -0.559017f;
normals.points[7].normal[0] = 0.769421f; normals.points[7].normal[1] = -0.309017f; normals.points[7].normal[2] = -0.559017f;
normals.points[8].normal[0] = -0.769421f; normals.points[8].normal[1] = 0.309017f; normals.points[8].normal[2] = -0.559017f;
normals.points[9].normal[0] = 0.769421f; normals.points[9].normal[1] = 0.309017f; normals.points[9].normal[2] = -0.559017f;
normals.points[10].normal[0] = -0.181636f; normals.points[10].normal[1] = 0.809017f; normals.points[10].normal[2] = -0.559017f;
normals.points[11].normal[0] = 0.181636f; normals.points[11].normal[1] = 0.809017f; normals.points[11].normal[2] = -0.559017f;
normals.points[12].normal[0] = -0.475528f; normals.points[12].normal[1] = -0.809017f; normals.points[12].normal[2] = -0.345491f;
normals.points[13].normal[0] = 0.475528f; normals.points[13].normal[1] = -0.809017f; normals.points[13].normal[2] = -0.345491f;
normals.points[14].normal[0] = -0.475528f; normals.points[14].normal[1] = 0.809017f; normals.points[14].normal[2] = -0.345491f;
normals.points[15].normal[0] = 0.475528f; normals.points[15].normal[1] = 0.809017f; normals.points[15].normal[2] = -0.345491f;
normals.points[16].normal[0] = -0.000000f; normals.points[16].normal[1] = -1.000000f; normals.points[16].normal[2] = 0.000000f;
normals.points[17].normal[0] = 0.000000f; normals.points[17].normal[1] = -1.000000f; normals.points[17].normal[2] = 0.000000f;
normals.points[18].normal[0] = -0.587785f; normals.points[18].normal[1] = -0.809017f; normals.points[18].normal[2] = 0.000000f;
normals.points[19].normal[0] = 0.587785f; normals.points[19].normal[1] = -0.809017f; normals.points[19].normal[2] = 0.000000f;
normals.points[20].normal[0] = -0.951057f; normals.points[20].normal[1] = -0.309017f; normals.points[20].normal[2] = 0.000000f;
normals.points[21].normal[0] = 0.951057f; normals.points[21].normal[1] = -0.309017f; normals.points[21].normal[2] = 0.000000f;
normals.points[22].normal[0] = -0.951057f; normals.points[22].normal[1] = 0.309017f; normals.points[22].normal[2] = 0.000000f;
normals.points[23].normal[0] = 0.951057f; normals.points[23].normal[1] = 0.309017f; normals.points[23].normal[2] = 0.000000f;
normals.points[24].normal[0] = -0.587785f; normals.points[24].normal[1] = 0.809017f; normals.points[24].normal[2] = 0.000000f;
normals.points[25].normal[0] = 0.587785f; normals.points[25].normal[1] = 0.809017f; normals.points[25].normal[2] = 0.000000f;
normals.points[26].normal[0] = 0.000000f; normals.points[26].normal[1] = 1.000000f; normals.points[26].normal[2] = 0.000000f;
// Create a shared sphere model pointer directly
SampleConsensusModelNormalSpherePtr model (new SampleConsensusModelNormalSphere<PointXYZ, Normal> (cloud.makeShared ()));
model->setInputNormals(normals.makeShared ());
// Create the RANSAC object
RandomSampleConsensus<PointXYZ> sac (model, 0.03);
// Algorithm tests
bool result = sac.computeModel ();
ASSERT_EQ (result, true);
std::vector<int> sample;
sac.getModel (sample);
EXPECT_EQ (int (sample.size ()), 4);
std::vector<int> inliers;
sac.getInliers (inliers);
EXPECT_EQ (int (inliers.size ()), 27);
Eigen::VectorXf coeff;
sac.getModelCoefficients (coeff);
EXPECT_EQ (int (coeff.size ()), 4);
EXPECT_NEAR (coeff[0], 0.0, 1e-2);
EXPECT_NEAR (coeff[1], 0.025, 1e-2);
EXPECT_NEAR (coeff[2], 1.0, 1e-2);
EXPECT_NEAR (coeff[3], 0.05, 1e-2);
Eigen::VectorXf coeff_refined;
model->optimizeModelCoefficients (inliers, coeff, coeff_refined);
EXPECT_EQ (int (coeff_refined.size ()), 4);
EXPECT_NEAR (coeff_refined[0], 0.0, 1e-2);
EXPECT_NEAR (coeff_refined[1], 0.025, 1e-2);
EXPECT_NEAR (coeff_refined[2], 1.0, 1e-2);
EXPECT_NEAR (coeff_refined[3], 0.05, 1e-2);
}
int main(int argc, char *argv[])
{
// Load a mesh
igl::readOBJ(TUTORIAL_SHARED_PATH "/inspired_mesh.obj", V, F);
printf("--Initialization--\n");
V_border = igl::is_border_vertex(V,F);
igl::adjacency_list(F, VV);
igl::vertex_triangle_adjacency(V,F,VF,VFi);
igl::triangle_triangle_adjacency(F,TT,TTi);
igl::edge_topology(V,F,E,F2E,E2F);
// Generate "subdivided" mesh for visualization of curl terms
igl::false_barycentric_subdivision(V, F, Vbs, Fbs);
// Compute scale for visualizing fields
global_scale = .2*igl::avg_edge_length(V, F);
//Compute scale for visualizing texture
uv_scale = 0.6/igl::avg_edge_length(V, F);
// Compute face barycenters
igl::barycenter(V, F, B);
// Compute local basis for faces
igl::local_basis(V,F,B1,B2,B3);
//Generate random vectors for constraints
generate_constraints();
// Interpolate a 2-PolyVector field to be used as the original field
printf("--Initial solution--\n");
igl::n_polyvector(V, F, b, bc, two_pv_ori);
// Post process original field
// Compute curl_minimizing matchings and curl
Eigen::MatrixXi match_ab, match_ba; // matchings across interior edges
printf("--Matchings and curl--\n");
double avgCurl = igl::polyvector_field_matchings(two_pv_ori, V, F, true, match_ab, match_ba, curl_ori);
double maxCurl = curl_ori.maxCoeff();
printf("original curl -- max: %.5g, avg: %.5g\n", maxCurl, avgCurl);
printf("--Singularities--\n");
// Compute singularities
igl::polyvector_field_singularities_from_matchings(V, F, V_border, VF, TT, E2F, F2E, match_ab, match_ba, singularities_ori);
printf("original #singularities: %ld\n", singularities.rows());
printf("--Cuts--\n");
// Get mesh cuts based on singularities
igl::polyvector_field_cut_mesh_with_singularities(V, F, VF, VV, TT, TTi, singularities_ori, cuts_ori);
printf("--Combing--\n");
// Comb field
Eigen::MatrixXd combed;
igl::polyvector_field_comb_from_matchings_and_cuts(V, F, TT, E2F, F2E, two_pv_ori, match_ab, match_ba, cuts_ori, combed);
printf("--Cut mesh--\n");
// Reconstruct integrable vector fields from combed field
igl::cut_mesh(V, F, VF, VFi, TT, TTi, V_border, cuts_ori, Vcut_ori, Fcut_ori);
printf("--Poisson--\n");
double avgPoisson = igl::polyvector_field_poisson_reconstruction(Vcut_ori, Fcut_ori, combed, scalars_ori, two_pv_poisson_ori, poisson_error_ori);
double maxPoisson = poisson_error_ori.maxCoeff();
printf("poisson error -- max: %.5g, avg: %.5g\n", maxPoisson, avgPoisson);
// Set the curl-free 2-PolyVector to equal the original field
two_pv = two_pv_ori;
singularities = singularities_ori;
curl = curl_ori;
cuts = cuts_ori;
two_pv_poisson = two_pv_poisson_ori;
poisson_error = poisson_error_ori;
Vcut = Vcut_ori;
Fcut = Fcut_ori;
scalars = scalars_ori;
printf("--Integrable - Precomputation--\n");
// Precompute stuff for solver
igl::integrable_polyvector_fields_precompute(V, F, b, bc, blevel, two_pv_ori, ipfdata);
cerr<<"Done. Press keys 1-0 for various visualizations, 'A' to improve integrability." <<endl;
igl::viewer::Viewer viewer;
viewer.callback_key_down = &key_down;
viewer.core.show_lines = false;
key_down(viewer,'2',0);
// Replace the standard texture with an integer shift invariant texture
line_texture(texture_R, texture_G, texture_B);
viewer.launch();
return 0;
}
TEST (RANSAC, SampleConsensusModelCylinder)
{
srand (0);
// Use a custom point cloud for these tests until we need something better
PointCloud<PointXYZ> cloud;
PointCloud<Normal> normals;
cloud.points.resize (20); normals.points.resize (20);
cloud.points[0].x = -0.499902f; cloud.points[0].y = 2.199701f; cloud.points[0].z = 0.000008f;
cloud.points[1].x = -0.875397f; cloud.points[1].y = 2.030177f; cloud.points[1].z = 0.050104f;
cloud.points[2].x = -0.995875f; cloud.points[2].y = 1.635973f; cloud.points[2].z = 0.099846f;
cloud.points[3].x = -0.779523f; cloud.points[3].y = 1.285527f; cloud.points[3].z = 0.149961f;
cloud.points[4].x = -0.373285f; cloud.points[4].y = 1.216488f; cloud.points[4].z = 0.199959f;
cloud.points[5].x = -0.052893f; cloud.points[5].y = 1.475973f; cloud.points[5].z = 0.250101f;
cloud.points[6].x = -0.036558f; cloud.points[6].y = 1.887591f; cloud.points[6].z = 0.299839f;
cloud.points[7].x = -0.335048f; cloud.points[7].y = 2.171994f; cloud.points[7].z = 0.350001f;
cloud.points[8].x = -0.745456f; cloud.points[8].y = 2.135528f; cloud.points[8].z = 0.400072f;
cloud.points[9].x = -0.989282f; cloud.points[9].y = 1.803311f; cloud.points[9].z = 0.449983f;
cloud.points[10].x = -0.900651f; cloud.points[10].y = 1.400701f; cloud.points[10].z = 0.500126f;
cloud.points[11].x = -0.539658f; cloud.points[11].y = 1.201468f; cloud.points[11].z = 0.550079f;
cloud.points[12].x = -0.151875f; cloud.points[12].y = 1.340951f; cloud.points[12].z = 0.599983f;
cloud.points[13].x = -0.000724f; cloud.points[13].y = 1.724373f; cloud.points[13].z = 0.649882f;
cloud.points[14].x = -0.188573f; cloud.points[14].y = 2.090983f; cloud.points[14].z = 0.699854f;
cloud.points[15].x = -0.587925f; cloud.points[15].y = 2.192257f; cloud.points[15].z = 0.749956f;
cloud.points[16].x = -0.927724f; cloud.points[16].y = 1.958846f; cloud.points[16].z = 0.800008f;
cloud.points[17].x = -0.976888f; cloud.points[17].y = 1.549655f; cloud.points[17].z = 0.849970f;
cloud.points[18].x = -0.702003f; cloud.points[18].y = 1.242707f; cloud.points[18].z = 0.899954f;
cloud.points[19].x = -0.289916f; cloud.points[19].y = 1.246296f; cloud.points[19].z = 0.950075f;
normals.points[0].normal[0] = 0.000098f; normals.points[0].normal[1] = 1.000098f; normals.points[0].normal[2] = 0.000008f;
normals.points[1].normal[0] = -0.750891f; normals.points[1].normal[1] = 0.660413f; normals.points[1].normal[2] = 0.000104f;
normals.points[2].normal[0] = -0.991765f; normals.points[2].normal[1] = -0.127949f; normals.points[2].normal[2] = -0.000154f;
normals.points[3].normal[0] = -0.558918f; normals.points[3].normal[1] = -0.829439f; normals.points[3].normal[2] = -0.000039f;
normals.points[4].normal[0] = 0.253627f; normals.points[4].normal[1] = -0.967447f; normals.points[4].normal[2] = -0.000041f;
normals.points[5].normal[0] = 0.894105f; normals.points[5].normal[1] = -0.447965f; normals.points[5].normal[2] = 0.000101f;
normals.points[6].normal[0] = 0.926852f; normals.points[6].normal[1] = 0.375543f; normals.points[6].normal[2] = -0.000161f;
normals.points[7].normal[0] = 0.329948f; normals.points[7].normal[1] = 0.943941f; normals.points[7].normal[2] = 0.000001f;
normals.points[8].normal[0] = -0.490966f; normals.points[8].normal[1] = 0.871203f; normals.points[8].normal[2] = 0.000072f;
normals.points[9].normal[0] = -0.978507f; normals.points[9].normal[1] = 0.206425f; normals.points[9].normal[2] = -0.000017f;
normals.points[10].normal[0] = -0.801227f; normals.points[10].normal[1] = -0.598534f; normals.points[10].normal[2] = 0.000126f;
normals.points[11].normal[0] = -0.079447f; normals.points[11].normal[1] = -0.996697f; normals.points[11].normal[2] = 0.000079f;
normals.points[12].normal[0] = 0.696154f; normals.points[12].normal[1] = -0.717889f; normals.points[12].normal[2] = -0.000017f;
normals.points[13].normal[0] = 0.998685f; normals.points[13].normal[1] = 0.048502f; normals.points[13].normal[2] = -0.000118f;
normals.points[14].normal[0] = 0.622933f; normals.points[14].normal[1] = 0.782133f; normals.points[14].normal[2] = -0.000146f;
normals.points[15].normal[0] = -0.175948f; normals.points[15].normal[1] = 0.984480f; normals.points[15].normal[2] = -0.000044f;
normals.points[16].normal[0] = -0.855476f; normals.points[16].normal[1] = 0.517824f; normals.points[16].normal[2] = 0.000008f;
normals.points[17].normal[0] = -0.953769f; normals.points[17].normal[1] = -0.300571f; normals.points[17].normal[2] = -0.000030f;
normals.points[18].normal[0] = -0.404035f; normals.points[18].normal[1] = -0.914700f; normals.points[18].normal[2] = -0.000046f;
normals.points[19].normal[0] = 0.420154f; normals.points[19].normal[1] = -0.907445f; normals.points[19].normal[2] = 0.000075f;
// Create a shared cylinder model pointer directly
SampleConsensusModelCylinderPtr model (new SampleConsensusModelCylinder<PointXYZ, Normal> (cloud.makeShared ()));
model->setInputNormals (normals.makeShared ());
// Create the RANSAC object
RandomSampleConsensus<PointXYZ> sac (model, 0.03);
// Algorithm tests
bool result = sac.computeModel ();
ASSERT_EQ (result, true);
std::vector<int> sample;
sac.getModel (sample);
EXPECT_EQ (int (sample.size ()), 2);
std::vector<int> inliers;
sac.getInliers (inliers);
EXPECT_EQ (int (inliers.size ()), 20);
Eigen::VectorXf coeff;
sac.getModelCoefficients (coeff);
EXPECT_EQ (int (coeff.size ()), 7);
EXPECT_NEAR (coeff[0], -0.5, 1e-3);
EXPECT_NEAR (coeff[1], 1.7, 1e-3);
EXPECT_NEAR (coeff[6], 0.5, 1e-3);
Eigen::VectorXf coeff_refined;
model->optimizeModelCoefficients (inliers, coeff, coeff_refined);
EXPECT_EQ (int (coeff_refined.size ()), 7);
EXPECT_NEAR (coeff_refined[6], 0.5, 1e-3);
}
virtual Eigen::VectorXf initialValue() {
Eigen::VectorXf p( unary_*initial_u_param_.rows() + pairwise_*initial_lbl_param_.rows() + kernel_*initial_knl_param_.rows() );
p << (unary_?initial_u_param_:Eigen::VectorXf()), (pairwise_?initial_lbl_param_:Eigen::VectorXf()), (kernel_?initial_knl_param_:Eigen::VectorXf());
return p;
}
TEST (RANSAC, SampleConsensusModelLine)
{
srand (0);
// Use a custom point cloud for these tests until we need something better
PointCloud<PointXYZ> cloud;
cloud.points.resize (10);
cloud.points[0].x = 1.0f; cloud.points[0].y = 2.0f; cloud.points[0].z = 3.0f;
cloud.points[1].x = 4.0f; cloud.points[1].y = 5.0f; cloud.points[1].z = 6.0f;
cloud.points[2].x = 7.0f; cloud.points[2].y = 8.0f; cloud.points[2].z = 9.0f;
cloud.points[3].x = 10.0f; cloud.points[3].y = 11.0f; cloud.points[3].z = 12.0f;
cloud.points[4].x = 13.0f; cloud.points[4].y = 14.0f; cloud.points[4].z = 15.0f;
cloud.points[5].x = 16.0f; cloud.points[5].y = 17.0f; cloud.points[5].z = 18.0f;
cloud.points[6].x = 19.0f; cloud.points[6].y = 20.0f; cloud.points[6].z = 21.0f;
cloud.points[7].x = 22.0f; cloud.points[7].y = 23.0f; cloud.points[7].z = 24.0f;
cloud.points[8].x = -5.0f; cloud.points[8].y = 1.57f; cloud.points[8].z = 0.75f;
cloud.points[9].x = 4.0f; cloud.points[9].y = 2.0f; cloud.points[9].z = 3.0f;
// Create a shared line model pointer directly
SampleConsensusModelLinePtr model (new SampleConsensusModelLine<PointXYZ> (cloud.makeShared ()));
// Create the RANSAC object
RandomSampleConsensus<PointXYZ> sac (model, 0.001);
// Algorithm tests
bool result = sac.computeModel ();
ASSERT_EQ (result, true);
std::vector<int> sample;
sac.getModel (sample);
EXPECT_EQ (int (sample.size ()), 2);
std::vector<int> inliers;
sac.getInliers (inliers);
EXPECT_EQ (int (inliers.size ()), 8);
Eigen::VectorXf coeff;
sac.getModelCoefficients (coeff);
EXPECT_EQ (int (coeff.size ()), 6);
EXPECT_NEAR (coeff[4]/coeff[3], 1, 1e-4);
EXPECT_NEAR (coeff[5]/coeff[3], 1, 1e-4);
Eigen::VectorXf coeff_refined;
model->optimizeModelCoefficients (inliers, coeff, coeff_refined);
EXPECT_EQ (int (coeff_refined.size ()), 6);
EXPECT_NEAR (coeff[4]/coeff[3], 1, 1e-4);
EXPECT_NEAR (coeff[5]/coeff[3], 1, 1e-4);
// Projection tests
PointCloud<PointXYZ> proj_points;
model->projectPoints (inliers, coeff_refined, proj_points);
EXPECT_NEAR (proj_points.points[2].x, 7.0, 1e-4);
EXPECT_NEAR (proj_points.points[2].y, 8.0, 1e-4);
EXPECT_NEAR (proj_points.points[2].z, 9.0, 1e-4);
EXPECT_NEAR (proj_points.points[3].x, 10.0, 1e-4);
EXPECT_NEAR (proj_points.points[3].y, 11.0, 1e-4);
EXPECT_NEAR (proj_points.points[3].z, 12.0, 1e-4);
EXPECT_NEAR (proj_points.points[5].x, 16.0, 1e-4);
EXPECT_NEAR (proj_points.points[5].y, 17.0, 1e-4);
EXPECT_NEAR (proj_points.points[5].z, 18.0, 1e-4);
}
virtual double gradient( const Eigen::VectorXf & x, Eigen::VectorXf & dx ) {
int p = 0;
if (unary_) {
crf_.setUnaryParameters( x.segment( p, initial_u_param_.rows() ) );
p += initial_u_param_.rows();
}
if (pairwise_) {
crf_.setLabelCompatibilityParameters( x.segment( p, initial_lbl_param_.rows() ) );
p += initial_lbl_param_.rows();
}
if (kernel_)
crf_.setKernelParameters( x.segment( p, initial_knl_param_.rows() ) );
Eigen::VectorXf du = 0*initial_u_param_, dl = 0*initial_u_param_, dk = 0*initial_knl_param_;
double r = crf_.gradient( NIT_, objective_, unary_?&du:NULL, pairwise_?&dl:NULL, kernel_?&dk:NULL );
dx.resize( unary_*du.rows() + pairwise_*dl.rows() + kernel_*dk.rows() );
dx << -(unary_?du:Eigen::VectorXf()), -(pairwise_?dl:Eigen::VectorXf()), -(kernel_?dk:Eigen::VectorXf());
r = -r;
if( l2_norm_ > 0 ) {
dx += l2_norm_ * x;
r += 0.5*l2_norm_ * (x.dot(x));
}
return r;
}
void Eigen_to_G(const Eigen::VectorXf& EigenV, Vector* _GV) {
Vector& GV = *_GV;
GV.Dimension(EigenV.size());
for (int i = 0; i < EigenV.size(); i++) GV[i] = EigenV(i);
}
int main( int argc, char* argv[]){
if (argc<4){
printf("Usage: %s image annotations output\n", argv[0] );
return 1;
}
// Number of labels [use only 4 to make our lives a bit easier]
const int M = 4;
// Load the color image and some crude annotations (which are used in a simple classifier)
int W, H, GW, GH;
unsigned char * im = readPPM( argv[1], W, H );
if (!im){
printf("Failed to load image!\n");
return 1;
}
unsigned char * anno = readPPM( argv[2], GW, GH );
if (!anno){
printf("Failed to load annotations!\n");
return 1;
}
if (W!=GW || H!=GH){
printf("Annotation size doesn't match image!\n");
return 1;
}
// Get the labeling
VectorXs labeling = getLabeling( anno, W*H, M );
const int N = W*H;
// Get the logistic features (unary term)
// Here we just use the color as a feature
Eigen::MatrixXf logistic_feature( 4, N ), logistic_transform( M, 4 );
logistic_feature.fill( 1.f );
for( int i=0; i<N; i++ )
for( int k=0; k<3; k++ )
logistic_feature(k,i) = im[3*i+k] / 255.;
for( int j=0; j<logistic_transform.cols(); j++ )
for( int i=0; i<logistic_transform.rows(); i++ )
logistic_transform(i,j) = 0.01*(1-2.*random()/RAND_MAX);
// Setup the CRF model
DenseCRF2D crf(W, H, M);
// Add a logistic unary term
crf.setUnaryEnergy( logistic_transform, logistic_feature );
// Add simple pairwise potts terms
crf.addPairwiseGaussian( 3, 3, new PottsCompatibility( 1 ) );
// Add a longer range label compatibility term
crf.addPairwiseBilateral( 80.0f, 80.0f, 13.0f, 13.0f, 13.0f, &*im, new MatrixCompatibility::MatrixCompatibility( Eigen::MatrixXf::Identity(M,M) ) );
// Choose your loss function
// LogLikelihood objective( labeling, 0.01 ); // Log likelihood loss
// Hamming objective( labeling, 0.0 ); // Global accuracy
// Hamming objective( labeling, 1.0 ); // Class average accuracy
// Hamming objective( labeling, 0.2 ); // Hamming loss close to intersection over union
IntersectionOverUnion objective( labeling ); // Intersection over union accuracy
int NIT = 5;
const bool verbose = true;
Eigen::MatrixXf learning_params( 3, 3 );
// Optimize the CRF in 3 phases:
// * First unary only
// * Unary and pairwise
// * Full CRF
learning_params<<1,0,0,
1,1,0,
1,1,1;
for( int i=0; i<learning_params.rows(); i++ ) {
// Setup the energy
CRFEnergy energy( crf, objective, NIT, learning_params(i,0), learning_params(i,1), learning_params(i,2) );
energy.setL2Norm( 1e-3 );
// Minimize the energy
Eigen::VectorXf p = minimizeLBFGS( energy, 2, true );
// Save the values
int id = 0;
if( learning_params(i,0) ) {
crf.setUnaryParameters( p.segment( id, crf.unaryParameters().rows() ) );
id += crf.unaryParameters().rows();
}
if( learning_params(i,1) ) {
crf.setLabelCompatibilityParameters( p.segment( id, crf.labelCompatibilityParameters().rows() ) );
id += crf.labelCompatibilityParameters().rows();
}
if( learning_params(i,2) )
crf.setKernelParameters( p.segment( id, crf.kernelParameters().rows() ) );
}
// Return the parameters
std::cout<<"Unary parameters: "<<crf.unaryParameters().transpose()<<std::endl;
std::cout<<"Pairwise parameters: "<<crf.labelCompatibilityParameters().transpose()<<std::endl;
std::cout<<"Kernel parameters: "<<crf.kernelParameters().transpose()<<std::endl;
// Do map inference
VectorXs map = crf.map(NIT);
// Store the result
unsigned char *res = colorize( map, W, H );
writePPM( argv[3], W, H, res );
delete[] im;
delete[] anno;
delete[] res;
}
template <typename PointT, typename PointNT> void
pcl::SampleConsensusModelCone<PointT, PointNT>::projectPoints (
const std::vector<int> &inliers, const Eigen::VectorXf &model_coefficients, PointCloud &projected_points, bool copy_data_fields)
{
// Needs a valid set of model coefficients
if (model_coefficients.size () != 7)
{
PCL_ERROR ("[pcl::SampleConsensusModelCone::projectPoints] Invalid number of model coefficients given (%lu)!\n", model_coefficients.size ());
return;
}
projected_points.header = input_->header;
projected_points.is_dense = input_->is_dense;
Eigen::Vector4f apex (model_coefficients[0], model_coefficients[1], model_coefficients[2], 0);
Eigen::Vector4f axis_dir (model_coefficients[3], model_coefficients[4], model_coefficients[5], 0);
float opening_angle = model_coefficients[6];
float apexdotdir = apex.dot (axis_dir);
float dirdotdir = 1.0f / axis_dir.dot (axis_dir);
// Copy all the data fields from the input cloud to the projected one?
if (copy_data_fields)
{
// Allocate enough space and copy the basics
projected_points.points.resize (input_->points.size ());
projected_points.width = input_->width;
projected_points.height = input_->height;
typedef typename pcl::traits::fieldList<PointT>::type FieldList;
// Iterate over each point
for (size_t i = 0; i < projected_points.points.size (); ++i)
// Iterate over each dimension
pcl::for_each_type <FieldList> (NdConcatenateFunctor <PointT, PointT> (input_->points[i], projected_points.points[i]));
// Iterate through the 3d points and calculate the distances from them to the cone
for (size_t i = 0; i < inliers.size (); ++i)
{
Eigen::Vector4f pt (input_->points[inliers[i]].x,
input_->points[inliers[i]].y,
input_->points[inliers[i]].z,
1);
float k = (pt.dot (axis_dir) - apexdotdir) * dirdotdir;
pcl::Vector4fMap pp = projected_points.points[inliers[i]].getVector4fMap ();
pp.matrix () = apex + k * axis_dir;
Eigen::Vector4f dir = pt - pp;
dir.normalize ();
// Calculate the actual radius of the cone at the level of the projected point
Eigen::Vector4f height = apex - pp;
float actual_cone_radius = tanf (opening_angle) * height.norm ();
// Calculate the projection of the point onto the cone
pp += dir * actual_cone_radius;
}
}
else
{
// Allocate enough space and copy the basics
projected_points.points.resize (inliers.size ());
projected_points.width = static_cast<uint32_t> (inliers.size ());
projected_points.height = 1;
typedef typename pcl::traits::fieldList<PointT>::type FieldList;
// Iterate over each point
for (size_t i = 0; i < inliers.size (); ++i)
// Iterate over each dimension
pcl::for_each_type <FieldList> (NdConcatenateFunctor <PointT, PointT> (input_->points[inliers[i]], projected_points.points[i]));
// Iterate through the 3d points and calculate the distances from them to the cone
for (size_t i = 0; i < inliers.size (); ++i)
{
pcl::Vector4fMap pp = projected_points.points[i].getVector4fMap ();
pcl::Vector4fMapConst pt = input_->points[inliers[i]].getVector4fMap ();
float k = (pt.dot (axis_dir) - apexdotdir) * dirdotdir;
// Calculate the projection of the point on the line
pp.matrix () = apex + k * axis_dir;
Eigen::Vector4f dir = pt - pp;
dir.normalize ();
// Calculate the actual radius of the cone at the level of the projected point
Eigen::Vector4f height = apex - pp;
float actual_cone_radius = tanf (opening_angle) * height.norm ();
// Calculate the projection of the point onto the cone
pp += dir * actual_cone_radius;
}
}
}
template <typename PointT, typename PointNT> bool
pcl::SampleConsensusModelCylinder<PointT, PointNT>::computeModelCoefficients (
const std::vector<int> &samples, Eigen::VectorXf &model_coefficients)
{
// Need 2 samples
if (samples.size () != 2)
{
PCL_ERROR ("[pcl::SampleConsensusModelCylinder::computeModelCoefficients] Invalid set of samples given (%zu)!\n", samples.size ());
return (false);
}
if (!normals_)
{
PCL_ERROR ("[pcl::SampleConsensusModelCylinder::computeModelCoefficients] No input dataset containing normals was given!\n");
return (false);
}
Eigen::Vector4f p1 (input_->points[samples[0]].x, input_->points[samples[0]].y, input_->points[samples[0]].z, 0);
Eigen::Vector4f p2 (input_->points[samples[1]].x, input_->points[samples[1]].y, input_->points[samples[1]].z, 0);
Eigen::Vector4f n1 (normals_->points[samples[0]].normal[0], normals_->points[samples[0]].normal[1], normals_->points[samples[0]].normal[2], 0);
Eigen::Vector4f n2 (normals_->points[samples[1]].normal[0], normals_->points[samples[1]].normal[1], normals_->points[samples[1]].normal[2], 0);
Eigen::Vector4f w = n1 + p1 - p2;
float a = n1.dot (n1);
float b = n1.dot (n2);
float c = n2.dot (n2);
float d = n1.dot (w);
float e = n2.dot (w);
float denominator = a*c - b*b;
float sc, tc;
// Compute the line parameters of the two closest points
if (denominator < 1e-8) // The lines are almost parallel
{
sc = 0.0f;
tc = (b > c ? d / b : e / c); // Use the largest denominator
}
else
{
sc = (b*e - c*d) / denominator;
tc = (a*e - b*d) / denominator;
}
// point_on_axis, axis_direction
Eigen::Vector4f line_pt = p1 + n1 + sc * n1;
Eigen::Vector4f line_dir = p2 + tc * n2 - line_pt;
line_dir.normalize ();
model_coefficients.resize (7);
// model_coefficients.template head<3> () = line_pt.template head<3> ();
model_coefficients[0] = line_pt[0];
model_coefficients[1] = line_pt[1];
model_coefficients[2] = line_pt[2];
// model_coefficients.template segment<3> (3) = line_dir.template head<3> ();
model_coefficients[3] = line_dir[0];
model_coefficients[4] = line_dir[1];
model_coefficients[5] = line_dir[2];
// cylinder radius
model_coefficients[6] = static_cast<float> (sqrt (pcl::sqrPointToLineDistance (p1, line_pt, line_dir)));
if (model_coefficients[6] > radius_max_ || model_coefficients[6] < radius_min_)
return (false);
return (true);
}
void SimDynamicsWindow::updateJointInfo()
{
//std::stringstream info;
std::string info;
int n = UI.comboBoxRobotNode->currentIndex();
QString qMin("0");
QString qMax("0");
QString qName("Name: <not set>");
QString qJV("Joint value: 0");
QString qTarget("Joint target: 0");
QString qVel("Joint velocity: 0");
QString qVelTarget("Joint velocity target: 0");
QString qGP("GlobalPose (simox): 0/0/0");
QString qVisu("VISU (simox): 0/0/0");
QString qCom("COM (bullet): 0/0/0");
QString tmp;
RobotNodeRevolutePtr rn;
if (n >= 0 && n < (int)robotNodes.size())
{
rn = robotNodes[n];
}
SimDynamics::DynamicsObjectPtr dynRN = dynamicsRobot->getDynamicsRobotNode(rn);
SimDynamics::BulletObjectPtr bulletRN = boost::dynamic_pointer_cast<SimDynamics::BulletObject>(dynRN);
if (bulletRN)
{
// cout << "FORCE: " << bulletRN->getRigidBody()->getTotalForce()[0] << ", " << bulletRN->getRigidBody()->getTotalForce()[1] << ", " << bulletRN->getRigidBody()->getTotalForce()[2] << endl;
// cout << "TORQUE: " << bulletRN->getRigidBody()->getTotalTorque()[0] << ", " << bulletRN->getRigidBody()->getTotalTorque()[1] << ", " << bulletRN->getRigidBody()->getTotalTorque()[2] << endl;
// cout << "getLinearVelocity: " << bulletRN->getRigidBody()->getLinearVelocity()[0] << ", " << bulletRN->getRigidBody()->getLinearVelocity()[1] << ", " << bulletRN->getRigidBody()->getLinearVelocity()[2] << endl;
// cout << "getAngularVelocity: " << bulletRN->getRigidBody()->getAngularVelocity()[0] << ", " << bulletRN->getRigidBody()->getAngularVelocity()[1] << ", " << bulletRN->getRigidBody()->getAngularVelocity()[2] << endl;
}
BulletRobotPtr bulletRobot = boost::dynamic_pointer_cast<SimDynamics::BulletRobot>(dynamicsRobot);
if (rn && bulletRobot && bulletRobot->hasLink(rn))
{
BulletRobot::LinkInfo linkInfo = bulletRobot->getLink(rn);
linkInfo.nodeA->showCoordinateSystem(true);
linkInfo.nodeB->showCoordinateSystem(true);
bulletRobot->enableForceTorqueFeedback(linkInfo);
Eigen::VectorXf ftA = bulletRobot->getForceTorqueFeedbackA(linkInfo);
Eigen::VectorXf ftB = bulletRobot->getForceTorqueFeedbackB(linkInfo);
std::stringstream streamFTA;
streamFTA << "ForceTorqueA: " << std::fixed;
streamFTA.precision(1);
for (int i = 0; i < 6; ++i)
{
streamFTA << ftA[i] << ",";
}
UI.label_ForceTorqueA->setText(QString::fromStdString(streamFTA.str()));
std::stringstream streamFTB;
streamFTB << "ForceTorqueB: " << std::fixed;
streamFTB.precision(1);
for (int i = 0; i < 6; ++i)
{
streamFTB << ftB[i] << ",";
}
UI.label_ForceTorqueB->setText(QString::fromStdString(streamFTB.str()));
// cout << "ForceTorqueA:" << endl;
// MathTools::print(ftA);
// cout << "ForceTorqueB:" << endl;
// MathTools::print(ftB);
forceSep->removeAllChildren();
SoUnits* u = new SoUnits();
u->units = SoUnits::MILLIMETERS;
forceSep->addChild(u);
Eigen::Vector3f n = ftA.head(3);
//n = linkInfo.nodeA->toGlobalCoordinateSystemVec(n);
float l = ftA.head(3).norm();
float w = 5.0f;
SoSeparator* forceA = new SoSeparator;
SoSeparator* arrowForceA = CoinVisualizationFactory::CreateArrow(n, l, w, VisualizationFactory::Color::Red());
/*
cout << "FORCE_A: " << linkInfo.dynNode1->getRigidBody()->getTotalForce()[0] << "," << linkInfo.dynNode1->getRigidBody()->getTotalForce()[1] << "," << linkInfo.dynNode1->getRigidBody()->getTotalForce()[2] << endl;
cout << "TORQUEA: " << linkInfo.dynNode1->getRigidBody()->getTotalTorque()[0] << "," << linkInfo.dynNode1->getRigidBody()->getTotalTorque()[1] << "," << linkInfo.dynNode1->getRigidBody()->getTotalTorque()[2] << endl;
cout << "FORCE_B: " << linkInfo.dynNode2->getRigidBody()->getTotalForce()[0] << "," << linkInfo.dynNode2->getRigidBody()->getTotalForce()[1] << "," << linkInfo.dynNode2->getRigidBody()->getTotalForce()[2] << endl;
cout << "TORQUEB: " << linkInfo.dynNode2->getRigidBody()->getTotalTorque()[0] << "," << linkInfo.dynNode2->getRigidBody()->getTotalTorque()[1] << "," << linkInfo.dynNode2->getRigidBody()->getTotalTorque()[2] << endl;
*/
// show as nodeA local coords system
/*
Eigen::Matrix4f comCoord = linkInfo.nodeA->getGlobalPose();
Eigen::Matrix4f comLocal = Eigen::Matrix4f::Identity();
comLocal.block(0,3,3,1) = linkInfo.nodeA->getCoMLocal();
comCoord = comCoord * comLocal;
*/
// show as global coords
Eigen::Matrix4f comGlobal = Eigen::Matrix4f::Identity();
comGlobal.block(0, 3, 3, 1) = linkInfo.nodeA->getCoMGlobal();
SoMatrixTransform* m = CoinVisualizationFactory::getMatrixTransform(comGlobal);
forceA->addChild(m);
forceA->addChild(arrowForceA);
forceSep->addChild(forceA);
Eigen::Vector3f jointGlobal = linkInfo.nodeJoint->getGlobalPose().block(0, 3, 3, 1);
Eigen::Vector3f comBGlobal = linkInfo.nodeB->getCoMGlobal();
// force that is applied on objectA by objectB
Eigen::Vector3f FBGlobal = ftA.head(3);
Eigen::Vector3f TBGlobal = ftB.tail(3) ;
Eigen::VectorXf torqueJointGlobal = bulletRobot->getJointForceTorqueGlobal(linkInfo);//= TBGlobal - (comBGlobal-jointGlobal).cross(FBGlobal) * 0.001;
// cout << "torqueJointGlobal: " << torqueJointGlobal << endl;
std::stringstream streamFTJoint;
streamFTJoint.precision(1);
streamFTJoint << "ForceTorqueJoint: " << std::fixed;
for (int i = 0; i < 6; ++i)
{
streamFTJoint << torqueJointGlobal[i] << ",";
}
UI.label_ForceTorqueJoint->setText(QString::fromStdString(streamFTJoint.str()));
n = ftB.head(3);
//n = linkInfo.nodeB->toGlobalCoordinateSystemVec(n);
l = ftB.head(3).norm();
w = 5.0f;
SoSeparator* forceB = new SoSeparator;
SoSeparator* arrowForceB = CoinVisualizationFactory::CreateArrow(n, l, w, VisualizationFactory::Color::Red());
comGlobal = Eigen::Matrix4f::Identity();
comGlobal.block(0, 3, 3, 1) = linkInfo.nodeB->getCoMGlobal();
m = CoinVisualizationFactory::getMatrixTransform(comGlobal);
forceB->addChild(m);
forceB->addChild(arrowForceB);
forceSep->addChild(forceB);
/*
if (!linkInfo.joint->needsFeedback())
{
linkInfo.joint->enableFeedback(true);
btJointFeedback* feedback = new btJointFeedback;
feedback->m_appliedForceBodyA = btVector3(0, 0, 0);
feedback->m_appliedForceBodyB = btVector3(0, 0, 0);
feedback->m_appliedTorqueBodyA = btVector3(0, 0, 0);
feedback->m_appliedTorqueBodyB = btVector3(0, 0, 0);
linkInfo.joint->setJointFeedback(feedback);
}
else
{
btJointFeedback* feedback = linkInfo.joint->getJointFeedback();
cout << "feedback->m_appliedForceBodyA: " << feedback->m_appliedForceBodyA[0] << "," << feedback->m_appliedForceBodyA[1] << "," << feedback->m_appliedForceBodyA[2] << endl;
cout << "feedback->m_appliedForceBodyB: " << feedback->m_appliedForceBodyB[0] << "," << feedback->m_appliedForceBodyB[1] << "," << feedback->m_appliedForceBodyB[2] << endl;
cout << "feedback->m_appliedTorqueBodyA: " << feedback->m_appliedTorqueBodyA[0] << "," << feedback->m_appliedTorqueBodyA[1] << "," << feedback->m_appliedTorqueBodyA[2] << endl;
cout << "feedback->m_appliedTorqueBodyB: " << feedback->m_appliedTorqueBodyB[0] << "," << feedback->m_appliedTorqueBodyB[1] << "," << feedback->m_appliedTorqueBodyB[2] << endl;
}
*/
}
if (rn)
{
qMin = QString::number(rn->getJointLimitLo(), 'f', 4);
qMax = QString::number(rn->getJointLimitHi(), 'f', 4);
qName = QString("Name: ");
qName += QString(rn->getName().c_str());
qJV = QString("Joint value: ");
tmp = QString::number(rn->getJointValue(), 'f', 3);
qJV += tmp;
info += "jv rn:";
std::string a1 = (const char*)tmp.toAscii();
info += a1;
qJV += QString(" / ");
if (dynamicsRobot->isNodeActuated(rn))
{
tmp = QString::number(dynamicsRobot->getJointAngle(rn), 'f', 3);
}
else
{
tmp = QString("-");
}
qJV += tmp;
info += ",\tjv bul:";
a1 = (const char*)tmp.toAscii();
info += a1;
qTarget = QString("Joint target: ");
if (dynamicsRobot->isNodeActuated(rn))
{
tmp = QString::number(dynamicsRobot->getNodeTarget(rn), 'f', 3);
}
else
{
tmp = QString("-");
}
qTarget += tmp;
info += std::string(",targ:");
a1 = (const char*)tmp.toAscii();
info += a1;
qVel = QString("Joint velocity: ");
if (dynamicsRobot->isNodeActuated(rn))
{
tmp = QString::number(dynamicsRobot->getJointSpeed(rn), 'f', 3);
}
else
{
tmp = QString("-");
}
qVel += tmp;
info += ",vel:";
a1 = (const char*)tmp.toAscii();
info += a1;
qVelTarget = QString("Joint velocity target: ");
if (dynamicsRobot->isNodeActuated(rn))
{
tmp = QString::number(dynamicsRobot->getJointTargetSpeed(rn), 'f', 3);
}
else
{
tmp = QString("-");
}
qVelTarget += tmp;
info += ",velTarget:";
a1 = (const char*)tmp.toAscii();
info += a1;
Eigen::Matrix4f gp = rn->getGlobalPose();
qGP = QString("GlobalPose (simox):");
tmp = QString::number(gp(0, 3), 'f', 2);
qGP += tmp;
info += ",gp:";
info += (const char*)tmp.toAscii();
qGP += QString("/");
tmp = QString::number(gp(1, 3), 'f', 2);
qGP += tmp;
info += "/";
info += (const char*)tmp.toAscii();
qGP += QString("/");
tmp = QString::number(gp(2, 3), 'f', 2);
qGP += tmp;
info += "/";
info += (const char*)tmp.toAscii();
gp = rn->getGlobalPose();
qVisu = QString("VISU (simox):");
qVisu += QString::number(gp(0, 3), 'f', 2);
qVisu += QString("/");
qVisu += QString::number(gp(1, 3), 'f', 2);
qVisu += QString("/");
qVisu += QString::number(gp(2, 3), 'f', 2);
if (dynamicsRobot->hasDynamicsRobotNode(rn))
{
gp = dynamicsRobot->getComGlobal(rn);
}
else
{
gp = Eigen::Matrix4f::Identity();
}
qCom = QString("COM (bullet):");
qCom += QString::number(gp(0, 3), 'f', 2);
qCom += QString("/");
qCom += QString::number(gp(1, 3), 'f', 2);
qCom += QString("/");
qCom += QString::number(gp(2, 3), 'f', 2);
}
UI.label_TargetMin->setText(qMin);
UI.label_TargetMax->setText(qMax);
UI.label_RNName->setText(qName);
UI.label_RNValue->setText(qJV);
UI.label_RNTarget->setText(qTarget);
UI.label_RNVelocity->setText(qVel);
UI.label_RNVelocityTarget->setText(qVelTarget);
UI.label_RNPosGP->setText(qGP);
UI.label_RNPosVisu->setText(qVisu);
UI.label_RNPosCom->setText(qCom);
#if 0
// print some joint info
if (viewer->engineRunning())
{
cout << info << endl;
}
#endif
}
inline void resize(Eigen::VectorXf & v,
vcl_size_t new_size)
{
v.resize(new_size);
}