void trk3dFeatures::trkSeq(const PointCloudT::Ptr &cloud2,
const PointCloudT::Ptr &keyPts2,
const float params[],
PointCloudT::Ptr &keyPts,
pcl::PointCloud<SHOT1344>::Ptr &Shot1344Cloud_1,
std::vector<u16> &matchIdx1, std::vector<u16> &matchIdx2,
std::vector<f32> &matchDist)
{
// construct descriptors
pcl::PointCloud<SHOT1344>::Ptr Shot1344Cloud_2(new pcl::PointCloud<SHOT1344>);
extractFeatures featureDetector;
Shot1344Cloud_2 = featureDetector.Shot1344Descriptor(cloud2, keyPts2, params);
// // match keypoints
// featureDetector.crossMatching(Shot1344Cloud_1, Shot1344Cloud_2,
// &matchIdx1, &matchIdx2, &matchDist);
// find the matching from desc_1 -> desc_2
std::cout<<"size of Shot1344Cloud_1 in trkSeq: "<<Shot1344Cloud_1->points.size()<<std::endl;
std::cout<<"size of Shot1344Cloud_2 in trkSeq: "<<Shot1344Cloud_2->points.size()<<std::endl;
featureDetector.matchKeyPts(Shot1344Cloud_1, Shot1344Cloud_2, &matchIdx2, &matchDist);
std::cout<<"size of matches in trkSeq: "<<matchIdx2.size()<<std::endl;
std::cout<<"size of matchDist in trkSeq: "<<matchDist.size()<<std::endl;
Shot1344Cloud_1.reset(new pcl::PointCloud<SHOT1344>);
keyPts->points.clear();
for(size_t i=0; i<matchIdx2.size();i++)
{
keyPts->push_back(keyPts2->points.at(matchIdx2[i]));
Shot1344Cloud_1->push_back(Shot1344Cloud_2->points.at(matchIdx2[i]));
}
}
void computeNeonVoxels(PointCloudT::Ptr in, PointCloudT::Ptr green, PointCloudT::Ptr orange) {
green->clear();
orange->clear();
//Point Cloud to store out neon cap
//PointCloudT::Ptr temp_neon_cloud (new PointCloudT);
for (int i = 0; i < in->points.size(); i++) {
unsigned int r, g, b;
r = in->points[i].r;
g = in->points[i].g;
b = in->points[i].b;
// Look for mostly neon value points
if (g > 175 && (r + b) < 150) {
green->push_back(in->points[i]);
}
else if(r > 200 && (g + b) < 150){
orange->push_back(in->points[i]);
}
}
}