bool reduce_measurement_g2o::find_transform(const color_keyframe::Ptr & fi,
const color_keyframe::Ptr & fj, Sophus::SE3f & t) {
std::vector<cv::KeyPoint> keypoints_i, keypoints_j;
pcl::PointCloud<pcl::PointXYZ> keypoints3d_i, keypoints3d_j;
cv::Mat descriptors_i, descriptors_j;
compute_features(fi->get_i(0), fi->get_d(0), fi->get_intrinsics(0), fd, de,
keypoints_i, keypoints3d_i, descriptors_i);
compute_features(fj->get_i(0), fj->get_d(0), fj->get_intrinsics(0), fd, de,
keypoints_j, keypoints3d_j, descriptors_j);
std::vector<cv::DMatch> matches, matches_filtered;
dm->match(descriptors_j, descriptors_i, matches);
Eigen::Affine3f transform;
std::vector<bool> inliers;
bool res = estimate_transform_ransac(keypoints3d_j, keypoints3d_i, matches,
100, 0.03 * 0.03, 20, transform, inliers);
t = Sophus::SE3f(transform.rotation(), transform.translation());
return res;
}
// global variables used: NONE
void
vtree_user::process_file(std::string& filename, std::vector<FeatureVector>& clouds, bool onlySaveClouds )
{
ROS_INFO("Processing file %s...", filename.c_str());
// load the file
pcl::PointCloud<PointT>::Ptr cloud(new pcl::PointCloud<PointT>);
pcl::io::loadPCDFile<PointT> (filename, *cloud);
// Compute the features
pcl::PointCloud<PointT>::Ptr keypoint_cloud ( new pcl::PointCloud<PointT> );
pcl::PointCloud<FeatureType>::Ptr feature_cloud( new pcl::PointCloud<FeatureType> );
compute_features( cloud, keypoint_cloud, feature_cloud );
// Rectify the historgram values to ensure they are in [0,100]
FeatureVector feature_vector;
for( pcl::PointCloud<FeatureType>::iterator iter = feature_cloud->begin();
iter != feature_cloud->end(); ++iter)
{
rectify_histogram( *iter );
feature_vector.push_back( FeatureHist( iter->histogram ) );
}
ROS_INFO("Keypoints extracted and %d Features computed", static_cast<int>(feature_cloud -> size()) );
if (!onlySaveClouds)
clouds.push_back( feature_vector );
}
keypoint_map::keypoint_map(cv::Mat & rgb, cv::Mat & depth,
Eigen::Affine3f & transform) {
init_feature_detector(fd, de, dm);
intrinsics << 525.0, 525.0, 319.5, 239.5;
std::vector<cv::KeyPoint> keypoints;
compute_features(rgb, depth, intrinsics, fd, de, keypoints, keypoints3d,
descriptors);
for (int keypoint_id = 0; keypoint_id < keypoints.size(); keypoint_id++) {
observation o;
o.cam_id = 0;
o.point_id = keypoint_id;
o.coord << keypoints[keypoint_id].pt.x, keypoints[keypoint_id].pt.y;
observations.push_back(o);
weights.push_back(1.0f);
keypoints3d[keypoint_id].getVector3fMap() = transform
* keypoints3d[keypoint_id].getVector3fMap();
}
camera_positions.push_back(transform);
rgb_imgs.push_back(rgb);
depth_imgs.push_back(depth);
}
pcl_tools::icp_result alp_align(PointCloudT::Ptr object, PointCloudT::Ptr scene, PointCloudT::Ptr object_aligned,
int max_iterations, int num_samples, float similarity_thresh, float max_corresp_dist, float inlier_frac, float leaf) {
FeatureCloudT::Ptr object_features (new FeatureCloudT);
FeatureCloudT::Ptr scene_features (new FeatureCloudT);
// Downsample
pcl::console::print_highlight ("Downsampling...\n");
pcl::VoxelGrid<PointNT> grid;
// const float leaf = 0.005f;
// const float leaf = in_leaf;
grid.setLeafSize (leaf, leaf, leaf);
grid.setInputCloud (object);
grid.filter (*object);
grid.setInputCloud (scene);
grid.filter (*scene);
compute_normals(object);
compute_normals(scene);
compute_features(object, object_features);
compute_features(scene, scene_features);
// Perform alignment
pcl::console::print_highlight ("Starting alignment...\n");
pcl::SampleConsensusPrerejective<PointNT,PointNT,FeatureT> align;
align.setInputSource (object);
align.setSourceFeatures (object_features);
align.setInputTarget (scene);
align.setTargetFeatures (scene_features);
align.setMaximumIterations (max_iterations); // Number of RANSAC iterations
align.setNumberOfSamples (num_samples); // Number of points to sample for generating/prerejecting a pose
align.setCorrespondenceRandomness (12); // Number of nearest features to use
align.setSimilarityThreshold (similarity_thresh); // Polygonal edge length similarity threshold
align.setMaxCorrespondenceDistance (max_corresp_dist); // Inlier threshold
align.setInlierFraction (inlier_frac); // Required inlier fraction for accepting a pose hypothesis
{
pcl::ScopeTime t("Alignment");
align.align (*object_aligned);
}
pcl_tools::icp_result result;
result.affine = Eigen::Affine3d(align.getFinalTransformation().cast<double>());
result.converged = align.hasConverged();
result.inliers = align.getInliers ().size ();
return result;
}
Eigen::Matrix4f KinectFushionApp::initial_alignment(PointCloudRgbPtr src_points, PointCloudRgbPtr dst_points)
{ // Compute the intial alignment
PointCloudRgbPtr keypoints[2];
LocalDescriptorsPtr local_descriptors[2];
compute_features(src_points, keypoints[0], local_descriptors[0]);
compute_features(dst_points, keypoints[1], local_descriptors[1]);
//param
double min_sample_dist = 0.05, max_correspondence_dist=0.02, nr_iters=500;
// Find the transform that roughly aligns the points
Eigen::Matrix4f tform = computeInitialAlignment (keypoints[0], local_descriptors[0], keypoints[1], local_descriptors[1],
min_sample_dist, max_correspondence_dist, nr_iters);
PCL_INFO("Computed initial alignment\n");
return tform;
}
// global variables used: NONE
void
vtree_user::process_file(std::string& filename, std::vector<FeatureVector>& clouds, bool onlySaveClouds )
{
ROS_INFO("[vtree_user] Processing file %s...", filename.c_str());
// Compute the features
FeatureVector feature_vector;
compute_features( filename, feature_vector);
if (!onlySaveClouds)
clouds.push_back( feature_vector );
}
int main(int argc, char **argv)
{
unsigned int size = DAT_SIZE;
unsigned int i;
featureset f;
int fd;
struct timeval start, end;
unsigned char *mydata;
glob_t gbuf;
if (argc != 2) {
fprintf(stderr, "usage: %s <dirname>\n", argv[0]);
return 1;
}
if ((mydata = (unsigned char *)malloc(size)) == NULL) {
fprintf(stderr, "mydata could not be accomodated!\n");
return 1;
}
glob(argv[1], 0, NULL, &gbuf);
init_featureset(mydata, size, &f);
fprintf(stdout, "mean\tvariance\tentropy\tpower\tfmean\tfvariance\n");
for (i = 0; (i < gbuf.gl_pathc); ++i) {
fd = open(gbuf.gl_pathv[i], O_RDONLY);
//fprintf(stdout, "%s\t", gbuf.gl_pathv[i]);
read(fd, mydata, size);
gettimeofday(&start, NULL);
compute_features(mydata, size, &f);
gettimeofday(&end, NULL);
normalize_features(&f, size);
fprintf(stdout, "%1.15e\t", f.mean);
fprintf(stdout, "%1.15e\t", f.variance);
fprintf(stdout, "%1.15e\t", f.entropy);
fprintf(stdout, "%1.15e\t", f.power);
fprintf(stdout, "%1.15e\t", f.fmean);
fprintf(stdout, "%1.15e\n", f.fvariance);
close(fd);
}
finit_featureset(&f);
free(mydata);
return 0;
}
void
vtree_color_user::match_list( const pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud_matrix_map,
std::vector<std::pair<float,std::string> > & match_names,
int num_match )
{
// Extract keypoint in the pointcloud
vt::Document full_doc;
compute_features( cloud_matrix_map, full_doc );
// Obtain the matches from the database
vt::Matches matches;
db->find(full_doc, num_match, matches);
match_names.clear();
for ( vt::Matches::iterator it = matches.begin(); it != matches.end(); ++it)
match_names.push_back( std::make_pair( it->score, documents_map[it->id]->name ));
}
bool split_clusters(const char *input_path,const char *cluster_path,const char *output_path,int num_features,int clip_size) {
DiskReadMda C; C.setPath(cluster_path);
Mda C2;
C2.allocate(C.N1(),C.N2());
for (int i=0; i<C.N2(); i++) {
C2.setValue(C.value(0,i),0,i);
C2.setValue(C.value(1,i),1,i);
}
int K=get_K(C);
int kk=1;
#pragma omp parallel for
for (int k=1; k<=K; k++) {
DiskReadMda X; X.setPath(input_path); //needed here for thread safety?
DiskReadMda C; C.setPath(cluster_path);
printf("k=%d/%d... ",k,K);
QList<int> times=get_times(C,k);
Mda clips;
printf("extract clips... ");
extract_clips(clips,X,times,clip_size);
Mda features;
printf("compute features... ");
compute_features(features,clips,num_features);
printf("isosplit... ");
QVector<int> labels0=isosplit(features);
printf("setting...\n");
int K0=get_max_k(labels0);
if (K0>1) printf("::: split into %d clusters\n",K0);
else printf("\n");
int jjj=0;
for (int bb=0; bb<C.N2(); bb++) {
if (C.value(2,bb)==k) {
C2.setValue(kk+labels0[jjj]-1,2,bb);
jjj++;
}
}
kk+=K0;
}
C2.write(output_path);
return true;
}
void
vtree_user::match_list( const Eigen::Matrix<float,4,Eigen::Dynamic>& cloud_matrix_map,
std::vector<std::pair<float,std::string> > & match_names,
int num_match )
{
// Extract keypoint in the pointcloud
ROS_INFO("Extracting keypoints and computing features! We have a cloud with %d points", static_cast<int>(cloud_matrix_map.cols()) );
vt::Document full_doc;
compute_features( cloud_matrix_map, full_doc );
// Obtain the matches from the database
vt::Matches matches;
db->find(full_doc, num_match, matches);
match_names.clear();
for ( vt::Matches::iterator it = matches.begin(); it != matches.end(); ++it)
match_names.push_back( std::make_pair( it->score, documents_map[it->id]->name ));
}
int main(int argc, char **argv)
{
struct svm_model *model;
struct svm_node *x;
int fd;
unsigned int i, j;
double v;
unsigned int size = DAT_SIZE;
unsigned char *mydata;
featureset f;
glob_t gbuf;
if (argc != 3) {
fprintf(stderr, "usage: %s <modelfile> <inputdir>\n", argv[0]);
return 1;
}
if ((model = svm_load_model(argv[1])) == 0) {
fprintf(stderr, "could not open model file %s\n", argv[1]);
return 1;
}
glob(argv[2], 0, NULL, &gbuf);
if ((mydata = (unsigned char *)malloc(size)) == NULL) {
fprintf(stderr, "mydata could not be accomodated!\n");
return 1;
}
init_featureset(&f, size);
x = (struct svm_node *)malloc(max_nr_attr * sizeof(struct svm_node));
fprintf(stdout,
"file\ttype\tmean\tvariance\tentropy\tpower\tfmean\tfvariance\n");
for (i = 0; (i < gbuf.gl_pathc); ++i) {
if ((fd = open(gbuf.gl_pathv[i], O_RDONLY)) == -1) {
fprintf(stderr, "could not open input file %s\n",
gbuf.gl_pathv[i]);
return 1;
}
read(fd, mydata, size);
compute_features(mydata, size, &f);
normalize_features(&f, size);
init_svmnode(&f, x);
v = svm_predict(model, x);
fprintf(stdout, "%s\t", gbuf.gl_pathv[i]);
fprintf(stdout, "%g\t", v);
for (j = 0; j < max_nr_attr; ++j)
fprintf(stdout, "%g\t", x[j].value);
fprintf(stdout, "\n");
close(fd);
}
finit_featureset(&f);
svm_destroy_model(model);
free(mydata);
free(x);
return 0;
}
void
vtree_user::match_list( pcl::PointCloud<PointT>::Ptr & point_cloud_in,
std::vector<std::pair<float,std::string> > & match_names,
std::vector<std::pair<float,std::string> > & cluster_match_names,
int num_match )
{
// Extract keypoint in the pointcloud
ROS_INFO("Extracting keypoints and computing features! We have a cloud with %d points", static_cast<int>(point_cloud_in->size()) );
pcl::PointCloud<PointT>::Ptr keypoint_cloud ( new pcl::PointCloud<PointT> );
pcl::PointCloud<FeatureType>::Ptr feature_cloud( new pcl::PointCloud<FeatureType> );
compute_features( point_cloud_in, keypoint_cloud, feature_cloud );
int num_feat = feature_cloud->size();
ROS_INFO("Done. %d features found", num_feat);
if( num_feat == 0)
{
ROS_INFO("The feature cloud is empty");
return;
}
// Rectify the historgram values to ensure they are in [0,100] and create a document
vt::Document full_doc;
for( pcl::PointCloud<FeatureType>::iterator iter = feature_cloud->begin();
iter != feature_cloud->end(); ++iter)
{
rectify_histogram( *iter );
full_doc.push_back(tree.quantize( FeatureHist( iter->histogram ) ));
}
// Cluster the keypoints in 2D
ANNpointArray ann_points;
ann_points = annAllocPts(num_feat, 3);
std::vector<KeypointExt*> extended_keypoints;
for( int i=0; i < num_feat; ++i )
{
if (enable_clustering)
{
ann_points[i][0] = keypoint_cloud->points[i].x;
ann_points[i][1] = keypoint_cloud->points[i].y;
ann_points[i][2] = keypoint_cloud->points[i].z;
}
extended_keypoints.push_back( new KeypointExt( feature_cloud->at(i), full_doc[i] ) );
}
int cluster_count = 0;
std::vector<int> cluster_sizes;
if (enable_clustering)
{
std::vector<int> membership(num_feat);
cluster_count = pcd_utils::cluster_points( ann_points, num_feat, membership,
radius_adaptation_r_max, radius_adaptation_r_min,
radius_adaptation_A, radius_adaptation_K );
cluster_sizes.resize(cluster_count, 0);
//cluster_sizes.assign(cluster_count, 0);
for (int i = 0; i < num_feat; ++i)
{
extended_keypoints[i]->cluster = membership[i];
++cluster_sizes[membership[i]];
}
delete[] ann_points;
}
if(DEBUG)
ROS_INFO_STREAM("Clusters found = " << cluster_count);
//*******************************************************************
// Obtain the matches from the database
vt::Matches matches;
db->find(full_doc, num_match, matches); // std::string documents_map[matches[i].id]->name; float matches[i].score,
match_names.clear();
for ( vt::Matches::iterator it = matches.begin(); it != matches.end(); ++it)
match_names.push_back( std::make_pair( it->score, documents_map[it->id]->name ));
if (enable_clustering)
{
// store in matches_map
std::map<uint32_t, float> matches_map;
for ( vt::Matches::iterator it = matches.begin(); it != matches.end(); ++it)
{
matches_map[it->id] = it->score;
}
// Calculates and accumulates scores for each cluster
for (int c = 0; c < cluster_count; ++c)
{
vt::Document cluster_doc;
vt::Matches cluster_matches;
for (int i = 0; i < num_feat; ++i)
if ( extended_keypoints[i]->cluster == static_cast<unsigned int>(c) )
cluster_doc.push_back(full_doc[i]);
if (cluster_doc.size() < static_cast<unsigned int>(min_cluster_size))
continue;
db->find(cluster_doc, num_match, cluster_matches);
if(DEBUG)
ROS_INFO_STREAM("Cluster " << c << "(size = " << cluster_doc.size() << "):");
//update_matches_map(cluster_matches, cluster_doc.size());
for ( vt::Matches::iterator it = cluster_matches.begin(); it != cluster_matches.end(); ++it)
{
matches_map[it->id] = it->score;
}
}
// Get the updated match names
cluster_match_names.clear();
for( std::map<uint32_t, float>::iterator iter = matches_map.begin();
iter != matches_map.end(); ++iter )
{
cluster_match_names.push_back( std::make_pair( iter->second, documents_map[iter->first]->name) );
}
// sort
std::sort( cluster_match_names.begin(), cluster_match_names.end() );
}
}
void BinaryMaskExtractor::extract(
cv::Mat &unary_features,
std::vector<double> &probs,
std::vector<std::vector<double> > &per_classifier_probs,
std::vector<std::pair<int, std::vector<cv::Point> > > &comps,
std::vector<MserElement> &all_elements
)
{
boost::timer::cpu_timer t;
t.start();
probs.clear();
per_classifier_probs.clear();
comps.clear();
all_elements.clear();
cv::Mat swt1, swt2;
compute_swt(_image_gray, swt1, swt2);
if (ConfigurationManager::instance()->verbose())
std::cout << "Computed SWT in: " << boost::timer::format(t.elapsed(), 5, "%w") << std::endl;
t.start();
std::vector<std::vector<cv::Point> > msers;
std::vector<cv::Vec4i> hierarchy;
cv::Mat contour_img;
_image_gray.copyTo(contour_img);
contour_img = 255 - contour_img;
cv::findContours(contour_img, msers, hierarchy, CV_RETR_CCOMP, CV_CHAIN_APPROX_NONE);
if (ConfigurationManager::instance()->verbose())
std::cout << "Extracted "
<< msers.size()
<< " Contours in "
<< boost::timer::format(t.elapsed(), 5, "%w") << std::endl;
if (ConfigurationManager::instance()->keep_unary_features())
unary_features = cv::Mat(msers.size(), N_UNARY_FEATURES, CV_32FC1, cv::Scalar(0.0));
all_elements.clear();
t.start();
std::vector<std::vector<cv::Point> > regions;
for (int i = 0; i >= 0; i = hierarchy[i][0]) {
cv::Mat img(_image_gray.rows, _image_gray.cols, CV_8UC1, cv::Scalar(0));
cv::drawContours(img, msers, i, cv::Scalar(255), CV_FILLED, 8, hierarchy, 1);
std::vector<cv::Point> r(get_pixel_list(img));
MserElement el(-1, -1, -1, r);
regions.push_back(r);
// really be bigger than 2x5 pixels -> otherwise it is no CC
if (is_component_invalid(_mask, el.get_bounding_rect())) {
probs.push_back(0.0);
per_classifier_probs.push_back(std::vector<double> (2,0.0));
} else {
compute_features(swt1, swt2, el);
compute_probs(el, probs, per_classifier_probs);
}
all_elements.push_back(el);
}
if (ConfigurationManager::instance()->verbose())
std::cout << "Classified CCs in "
<< boost::timer::format(t.elapsed(), 5, "%w") << std::endl;
std::vector<std::vector<cv::Point> > components = regions;
comps.clear();
comps.reserve(components.size());
for (size_t i = 0; i < components.size(); i++) {
comps.push_back(std::make_pair(i + _uid_offset, components[i]));
}
}
int main(int argc, char ** argv)
{
clock_t t0;
t0 = clock();
bool print = true;
if (argc==1)
{
help();
exit(0);
}
std::string cmd(argv[1]);
//primitive programs that do not require help pages and summary statistics by default
if (argc>1 && cmd=="view")
{
print = view(argc-1, ++argv);
}
else if (argc>1 && cmd=="index")
{
print = index(argc-1, ++argv);
}
else if (argc>1 && cmd=="merge")
{
print = merge(argc-1, ++argv);
}
else if (argc>1 && cmd=="paste")
{
print = paste(argc-1, ++argv);
}
else if (argc>1 && cmd=="concat")
{
print = concat(argc-1, ++argv);
}
else if (argc>1 && cmd=="subset")
{
subset(argc-1, ++argv);
}
else if (argc>1 && cmd=="decompose")
{
decompose(argc-1, ++argv);
}
else if (argc>1 && cmd=="normalize")
{
print = normalize(argc-1, ++argv);
}
else if (argc>1 && cmd=="config")
{
config(argc-1, ++argv);
}
else if (argc>1 && cmd=="mergedups")
{
merge_duplicate_variants(argc-1, ++argv);
}
else if (argc>1 && cmd=="remove_overlap")
{
remove_overlap(argc-1, ++argv);
}
else if (argc>1 && cmd=="peek")
{
peek(argc-1, ++argv);
}
else if (argc>1 && cmd=="partition")
{
partition(argc-1, ++argv);
}
else if (argc>1 && cmd=="annotate_variants")
{
annotate_variants(argc-1, ++argv);
}
else if (argc>1 && cmd=="annotate_regions")
{
annotate_regions(argc-1, ++argv);
}
else if (argc>1 && cmd=="annotate_dbsnp_rsid")
{
annotate_dbsnp_rsid(argc-1, ++argv);
}
else if (argc>1 && cmd=="discover")
{
discover(argc-1, ++argv);
}
else if (argc>1 && cmd=="merge_candidate_variants")
{
merge_candidate_variants(argc-1, ++argv);
}
else if (argc>1 && cmd=="union_variants")
{
union_variants(argc-1, ++argv);
}
else if (argc>1 && cmd=="genotype")
{
genotype2(argc-1, ++argv);
}
else if (argc>1 && cmd=="characterize")
{
genotype(argc-1, ++argv);
}
else if (argc>1 && cmd=="construct_probes")
{
construct_probes(argc-1, ++argv);
}
else if (argc>1 && cmd=="profile_indels")
{
profile_indels(argc-1, ++argv);
}
else if (argc>1 && cmd=="profile_snps")
{
profile_snps(argc-1, ++argv);
}
else if (argc>1 && cmd=="profile_mendelian")
{
profile_mendelian(argc-1, ++argv);
}
else if (argc>1 && cmd=="profile_na12878")
{
profile_na12878(argc-1, ++argv);
}
else if (argc>1 && cmd=="profile_chrom")
{
profile_chrom(argc-1, ++argv);
}
else if (argc>1 && cmd=="align")
{
align(argc-1, ++argv);
}
else if (argc>1 && cmd=="compute_features")
{
compute_features(argc-1, ++argv);
}
else if (argc>1 && cmd=="profile_afs")
{
profile_afs(argc-1, ++argv);
}
else if (argc>1 && cmd=="profile_hwe")
{
profile_hwe(argc-1, ++argv);
}
else if (argc>1 && cmd=="profile_len")
{
profile_len(argc-1, ++argv);
}
else if (argc>1 && cmd=="annotate_str")
{
annotate_str(argc-1, ++argv);
}
else if (argc>1 && cmd=="consolidate_variants")
{
consolidate_variants(argc-1, ++argv);
}
else
{
std::clog << "Command not found: " << argv[1] << "\n\n";
help();
exit(1);
}
if (print)
{
clock_t t1;
t1 = clock();
print_time((float)(t1-t0)/CLOCKS_PER_SEC);
}
return 0;
}