Classes | Defines | Functions | Variables
frir_freehand.cpp File Reference
#include <ros/ros.h>
#include <pcl/point_types.h>
#include <pcl/point_cloud.h>
#include <pcl/ros/conversions.h>
#include <sensor_msgs/PointCloud2.h>
#include <pcl/ModelCoefficients.h>
#include <pcl/io/pcd_io.h>
#include <pcl/sample_consensus/method_types.h>
#include <pcl/sample_consensus/model_types.h>
#include <pcl/segmentation/sac_segmentation.h>
#include <pcl/segmentation/extract_clusters.h>
#include <pcl/filters/extract_indices.h>
#include <pcl/kdtree/kdtree.h>
#include <pcl/filters/voxel_grid.h>
#include <pcl/features/normal_3d_omp.h>
#include <pcl/filters/project_inliers.h>
#include <pcl/surface/concave_hull.h>
#include <pcl/common/pca.h>
#include <pcl/registration/registration.h>
#include <boost/timer.hpp>
#include <pcl/filters/statistical_outlier_removal.h>
#include <pcl/common/distances.h>
#include <pcl/registration/icp.h>
#include "pcl/sample_consensus/ransac.h"
#include "pcl/sample_consensus/sac_model_registration.h"
#include <visualization_msgs/Marker.h>
#include "std_msgs/String.h"
#include <pcl/common/transformation_from_correspondences.h>
#include <tf/transform_broadcaster.h>
#include <pcl/visualization/cloud_viewer.h>
#include <pcl/registration/correspondence_estimation.h>
#include <pcl/surface/mls.h>
#include <registration/general_registration.h>
#include <registration/registration_info.h>
#include <boost/thread.hpp>
Include dependency graph for frir_freehand.cpp:

Go to the source code of this file.

Classes

class  calcTF< Point >
struct  COR_S
struct  TestNode::DualFloat
struct  TestNode::Polyline
class  PrecisionStopWatch
struct  SORT_S
struct  SORT_S2
class  TestNode
class  TestNode2

Defines

#define _FN1_   "/home/josh/tmp/1305031229.729077.png.img.pcd"
#define _FN2_   "/home/josh/tmp/1305031229.764869.png.img.pcd"
#define _SEARCH_   2
#define DEBUG_SWITCH_   0
#define DIS_TREE_
#define GET_MIDS()
#define getInd(x, y)   ((x)+(y)*pc.width)
#define MAP_
#define MY_POINT   pcl::PointXYZRGB
#define OCTOMAP_   0
#define OPT_SIZE_   2
#define RGB_
#define TESTING_   0
#define USE_SMART_GRID_   0
#define USE_TF   1
#define WHOLE_DEPTH_

Functions

template<typename Point >
Eigen::Matrix4f _findTF (const pcl::PointCloud< Point > &pc_old, const pcl::PointCloud< Point > &pc_new, std::vector< COR_S > &cors, float border_a, float border_t, float border_a2, std::vector< int > &weight_o, std::vector< int > &weight_n, const Eigen::Vector3f &prev_trans=Null3f())
template<typename Point >
float avgError (const pcl::PointCloud< Point > &pc_old, const pcl::PointCloud< Point > &pc_new, const std::vector< COR_S > &cors, const Eigen::Matrix4f &tf=Eigen::Matrix4f::Identity())
Eigen::Matrix4f findCorr3_tf (const pcl::PointCloud< MY_POINT > &pc_old, const pcl::PointCloud< MY_POINT > &pc_new, std::vector< int > inds_old, std::vector< int > inds, const int i_sz_old, const int i_sz)
template<typename Point >
Eigen::Matrix4f findTF (const pcl::PointCloud< Point > &pc_old, const pcl::PointCloud< Point > &pc_new, const float rmax=0.1, const float tmax=0.1, Eigen::Matrix4f tf=Eigen::Matrix4f::Identity())
template<typename Point >
Eigen::Matrix4f findTF_fast (const pcl::PointCloud< Point > &pc_old, const pcl::PointCloud< Point > &pc_new, const float rmax=0.1, const float tmax=0.1, Eigen::Matrix4f tf=Eigen::Matrix4f::Identity())
template<typename Point >
Eigen::Vector3f getAxis (const pcl::PointCloud< Point > &pc_old, const pcl::PointCloud< Point > &pc_new, const std::vector< COR_S > &cors, const bool sm, const float max_dis, const Eigen::Vector3f &rel_old, const Eigen::Vector3f &rel_new)
template<typename Point >
Eigen::Vector3f getAxis2 (const pcl::PointCloud< Point > &pc_old, const pcl::PointCloud< Point > &pc_new, std::vector< COR_S > &cors, const float max_dis, const Eigen::Vector3f &rel_old, const Eigen::Vector3f &rel_new, std::vector< int > &weight_o, std::vector< int > &weight_n)
template<typename Point >
Eigen::Vector3f getAxis3 (const pcl::PointCloud< Point > &pc_old, const pcl::PointCloud< Point > &pc_new, const std::vector< COR_S > &cors, const float max_dis, const Eigen::Vector3f &rel_old, const Eigen::Vector3f &rel_new, std::vector< int > &weight_o, std::vector< int > &weight_n, const Eigen::Vector3f &X2)
void getDis (const Eigen::Vector3f &C, const Eigen::Vector3f &S, float &dT, float &dA)
template<typename Point >
Eigen::Vector3f getMidN (const pcl::PointCloud< Point > &pc, const std::vector< int > &weight, const bool sm, const float max_dis, const pcl::PointCloud< Point > &old, const std::vector< COR_S > &cors)
template<typename Point >
Eigen::Vector3f getMidO (const pcl::PointCloud< Point > &pc, const std::vector< int > &weight, const std::vector< COR_S > &cors, const bool sm=true, const float max_dis=10000000)
Eigen::Matrix4f getTF (const Eigen::Vector3f &p1, const Eigen::Vector3f &p2, const Eigen::Vector3f &p1n, const Eigen::Vector3f &p2n)
Eigen::Matrix4f getTF2 (const Eigen::Vector3f &axis, const Eigen::Vector3f &p1, const Eigen::Vector3f &p2, const Eigen::Vector3f &p_s, const Eigen::Vector3f &p1n, const Eigen::Vector3f &p2n, const Eigen::Vector3f &pn_s)
float getTF2dis (const Eigen::Vector3f &axis, const Eigen::Vector3f &p1, const Eigen::Vector3f &p2, const Eigen::Vector3f &p1n, const Eigen::Vector3f &p2n)
float getTF2dis2 (const Eigen::Vector3f &axis, const Eigen::Vector3f &p1, const Eigen::Vector3f &p2, const Eigen::Vector3f &p1n, const Eigen::Vector3f &p2n)
template<typename Point >
Eigen::Matrix4f getTF2ex (const Eigen::Vector3f &axis, const pcl::PointCloud< Point > &pc_old, const pcl::PointCloud< Point > &pc_new, const std::vector< COR_S > &cors, const std::vector< int > &weight_o, const std::vector< int > &weight_n, const Eigen::Vector3f &mid_old, const Eigen::Vector3f &mid_new)
Eigen::Matrix4f getTF3 (const Eigen::Vector3f &p1, const Eigen::Vector3f &p2, const Eigen::Vector3f &p3, const Eigen::Vector3f &p1n, const Eigen::Vector3f &p2n, const Eigen::Vector3f &p3n)
int main (int argc, char **argv)
bool makes_sense (const float f)
Eigen::Vector3f Null3f ()
template<typename Point >
int remByAxis (const pcl::PointCloud< Point > &pc_old, const pcl::PointCloud< Point > &pc_new, std::vector< COR_S > &cors, const Eigen::Vector3f &rel_old, const Eigen::Vector3f &rel_new, const Eigen::Vector3f &axis, const float add, std::vector< int > &weight_o, std::vector< int > &weight_n)
template<typename Point >
int remError (const pcl::PointCloud< Point > &pc_old, const pcl::PointCloud< Point > &pc_new, std::vector< COR_S > &cors, const Eigen::Matrix4f &tf, const float border_a, const float border_t, std::vector< int > &weight_o, std::vector< int > &weight_n, const Eigen::Vector3f &mid_old, const Eigen::Vector3f &mid_new)
int search_sorted_vector (const std::vector< SORT_S2 > &tv, const float val)
pcl::PointXYZ vec3TOxyz (const Eigen::Vector3f &v)
void visualize (const pcl::PointCloud< pcl::PointXYZ > &pc_old, const pcl::PointCloud< pcl::PointXYZ > &pc_new, const std::vector< COR_S > &cors, Eigen::Vector3f mo=Null3f(), Eigen::Vector3f mn=Null3f(), Eigen::Vector3f mot=Null3f())
void visualize (const pcl::PointCloud< pcl::PointXYZ > &pc_old, const pcl::PointCloud< pcl::PointXYZ > &pc_new, const std::vector< COR_S > &cors, const std::vector< int > &weight_o, const std::vector< int > &weight_n)
void visualize (const pcl::PointCloud< pcl::PointXYZRGB > &pc_old)
float vorz (float v)

Variables

bool g_step = false
std_msgs::Header header_
ros::Publisher map_pub_
ros::Publisher marker_pub_
ros::Publisher point_cloud_pub_

Define Documentation

#define _FN1_   "/home/josh/tmp/1305031229.729077.png.img.pcd"

Definition at line 11 of file frir_freehand.cpp.

#define _FN2_   "/home/josh/tmp/1305031229.764869.png.img.pcd"

Definition at line 12 of file frir_freehand.cpp.

#define _SEARCH_   2

Definition at line 4231 of file frir_freehand.cpp.

#define DEBUG_SWITCH_   0

Definition at line 64 of file frir_freehand.cpp.

#define DIS_TREE_

Definition at line 74 of file frir_freehand.cpp.

#define GET_MIDS ( )
Value:
\
    mid_old = getMidO(pc_old,weight_o,cors);\
    mid_new = getMidO(pc_new,weight_o,cors);\
    \
    mid_ob = getMidO(pc_old,weight_o,cors, true, mid_old.squaredNorm());\
    mid_nb = getMidN(pc_new,weight_o, true, mid_old.squaredNorm(), pc_old,cors);\
    \
    mid_oa = getMidO(pc_old,weight_o,cors, false, mid_old.squaredNorm());\
    mid_na = getMidN(pc_new,weight_o, false, mid_old.squaredNorm(), pc_old,cors);\
    \
    mid_old = 0.5f*(mid_ob+mid_oa);\
    mid_new = 0.5f*(mid_nb+mid_na);

Definition at line 1380 of file frir_freehand.cpp.

#define getInd (   x,
  y 
)    ((x)+(y)*pc.width)

Definition at line 3233 of file frir_freehand.cpp.

#define MAP_

Definition at line 71 of file frir_freehand.cpp.

#define MY_POINT   pcl::PointXYZRGB

Definition at line 112 of file frir_freehand.cpp.

#define OCTOMAP_   0

Definition at line 5055 of file frir_freehand.cpp.

#define OPT_SIZE_   2

Definition at line 75 of file frir_freehand.cpp.

#define RGB_

Definition at line 72 of file frir_freehand.cpp.

#define TESTING_   0

Definition at line 78 of file frir_freehand.cpp.

#define USE_SMART_GRID_   0

Definition at line 77 of file frir_freehand.cpp.

#define USE_TF   1

Definition at line 62 of file frir_freehand.cpp.

#define WHOLE_DEPTH_

Definition at line 73 of file frir_freehand.cpp.

Function Documentation

template<typename Point >
Eigen::Matrix4f _findTF ( const pcl::PointCloud< Point > &  pc_old,
const pcl::PointCloud< Point > &  pc_new,
std::vector< COR_S > &  cors,
float  border_a,
float  border_t,
float  border_a2,
std::vector< int > &  weight_o,
std::vector< int > &  weight_n,
const Eigen::Vector3f &  prev_trans = Null3f() 
)

Definition at line 1395 of file frir_freehand.cpp.

template<typename Point >
float avgError ( const pcl::PointCloud< Point > &  pc_old,
const pcl::PointCloud< Point > &  pc_new,
const std::vector< COR_S > &  cors,
const Eigen::Matrix4f &  tf = Eigen::Matrix4f::Identity() 
)

Definition at line 1205 of file frir_freehand.cpp.

Eigen::Matrix4f findCorr3_tf ( const pcl::PointCloud< MY_POINT > &  pc_old,
const pcl::PointCloud< MY_POINT > &  pc_new,
std::vector< int >  inds_old,
std::vector< int >  inds,
const int  i_sz_old,
const int  i_sz 
)

Definition at line 2523 of file frir_freehand.cpp.

template<typename Point >
Eigen::Matrix4f findTF ( const pcl::PointCloud< Point > &  pc_old,
const pcl::PointCloud< Point > &  pc_new,
const float  rmax = 0.1,
const float  tmax = 0.1,
Eigen::Matrix4f  tf = Eigen::Matrix4f::Identity() 
)

Definition at line 1741 of file frir_freehand.cpp.

template<typename Point >
Eigen::Matrix4f findTF_fast ( const pcl::PointCloud< Point > &  pc_old,
const pcl::PointCloud< Point > &  pc_new,
const float  rmax = 0.1,
const float  tmax = 0.1,
Eigen::Matrix4f  tf = Eigen::Matrix4f::Identity() 
)

Definition at line 1659 of file frir_freehand.cpp.

template<typename Point >
Eigen::Vector3f getAxis ( const pcl::PointCloud< Point > &  pc_old,
const pcl::PointCloud< Point > &  pc_new,
const std::vector< COR_S > &  cors,
const bool  sm,
const float  max_dis,
const Eigen::Vector3f &  rel_old,
const Eigen::Vector3f &  rel_new 
)

Definition at line 442 of file frir_freehand.cpp.

template<typename Point >
Eigen::Vector3f getAxis2 ( const pcl::PointCloud< Point > &  pc_old,
const pcl::PointCloud< Point > &  pc_new,
std::vector< COR_S > &  cors,
const float  max_dis,
const Eigen::Vector3f &  rel_old,
const Eigen::Vector3f &  rel_new,
std::vector< int > &  weight_o,
std::vector< int > &  weight_n 
)

Definition at line 622 of file frir_freehand.cpp.

template<typename Point >
Eigen::Vector3f getAxis3 ( const pcl::PointCloud< Point > &  pc_old,
const pcl::PointCloud< Point > &  pc_new,
const std::vector< COR_S > &  cors,
const float  max_dis,
const Eigen::Vector3f &  rel_old,
const Eigen::Vector3f &  rel_new,
std::vector< int > &  weight_o,
std::vector< int > &  weight_n,
const Eigen::Vector3f &  X2 
)

Definition at line 468 of file frir_freehand.cpp.

void getDis ( const Eigen::Vector3f &  C,
const Eigen::Vector3f &  S,
float &  dT,
float &  dA 
) [inline]

Definition at line 139 of file frir_freehand.cpp.

template<typename Point >
Eigen::Vector3f getMidN ( const pcl::PointCloud< Point > &  pc,
const std::vector< int > &  weight,
const bool  sm,
const float  max_dis,
const pcl::PointCloud< Point > &  old,
const std::vector< COR_S > &  cors 
)

Definition at line 174 of file frir_freehand.cpp.

template<typename Point >
Eigen::Vector3f getMidO ( const pcl::PointCloud< Point > &  pc,
const std::vector< int > &  weight,
const std::vector< COR_S > &  cors,
const bool  sm = true,
const float  max_dis = 10000000 
)

Definition at line 145 of file frir_freehand.cpp.

Eigen::Matrix4f getTF ( const Eigen::Vector3f &  p1,
const Eigen::Vector3f &  p2,
const Eigen::Vector3f &  p1n,
const Eigen::Vector3f &  p2n 
)

Definition at line 1003 of file frir_freehand.cpp.

Eigen::Matrix4f getTF2 ( const Eigen::Vector3f &  axis,
const Eigen::Vector3f &  p1,
const Eigen::Vector3f &  p2,
const Eigen::Vector3f &  p_s,
const Eigen::Vector3f &  p1n,
const Eigen::Vector3f &  p2n,
const Eigen::Vector3f &  pn_s 
)

Definition at line 1090 of file frir_freehand.cpp.

float getTF2dis ( const Eigen::Vector3f &  axis,
const Eigen::Vector3f &  p1,
const Eigen::Vector3f &  p2,
const Eigen::Vector3f &  p1n,
const Eigen::Vector3f &  p2n 
)

Definition at line 1058 of file frir_freehand.cpp.

float getTF2dis2 ( const Eigen::Vector3f &  axis,
const Eigen::Vector3f &  p1,
const Eigen::Vector3f &  p2,
const Eigen::Vector3f &  p1n,
const Eigen::Vector3f &  p2n 
)

Definition at line 1071 of file frir_freehand.cpp.

template<typename Point >
Eigen::Matrix4f getTF2ex ( const Eigen::Vector3f &  axis,
const pcl::PointCloud< Point > &  pc_old,
const pcl::PointCloud< Point > &  pc_new,
const std::vector< COR_S > &  cors,
const std::vector< int > &  weight_o,
const std::vector< int > &  weight_n,
const Eigen::Vector3f &  mid_old,
const Eigen::Vector3f &  mid_new 
)

Definition at line 1142 of file frir_freehand.cpp.

Eigen::Matrix4f getTF3 ( const Eigen::Vector3f &  p1,
const Eigen::Vector3f &  p2,
const Eigen::Vector3f &  p3,
const Eigen::Vector3f &  p1n,
const Eigen::Vector3f &  p2n,
const Eigen::Vector3f &  p3n 
)

Definition at line 985 of file frir_freehand.cpp.

int main ( int  argc,
char **  argv 
)

Definition at line 5301 of file frir_freehand.cpp.

bool makes_sense ( const float  f) [inline]

Definition at line 110 of file frir_freehand.cpp.

Eigen::Vector3f Null3f ( ) [inline]

Definition at line 133 of file frir_freehand.cpp.

template<typename Point >
int remByAxis ( const pcl::PointCloud< Point > &  pc_old,
const pcl::PointCloud< Point > &  pc_new,
std::vector< COR_S > &  cors,
const Eigen::Vector3f &  rel_old,
const Eigen::Vector3f &  rel_new,
const Eigen::Vector3f &  axis,
const float  add,
std::vector< int > &  weight_o,
std::vector< int > &  weight_n 
)

Definition at line 927 of file frir_freehand.cpp.

template<typename Point >
int remError ( const pcl::PointCloud< Point > &  pc_old,
const pcl::PointCloud< Point > &  pc_new,
std::vector< COR_S > &  cors,
const Eigen::Matrix4f &  tf,
const float  border_a,
const float  border_t,
std::vector< int > &  weight_o,
std::vector< int > &  weight_n,
const Eigen::Vector3f &  mid_old,
const Eigen::Vector3f &  mid_new 
)

Definition at line 1218 of file frir_freehand.cpp.

int search_sorted_vector ( const std::vector< SORT_S2 > &  tv,
const float  val 
)

Definition at line 1641 of file frir_freehand.cpp.

pcl::PointXYZ vec3TOxyz ( const Eigen::Vector3f &  v) [inline]

Definition at line 977 of file frir_freehand.cpp.

void visualize ( const pcl::PointCloud< pcl::PointXYZ > &  pc_old,
const pcl::PointCloud< pcl::PointXYZ > &  pc_new,
const std::vector< COR_S > &  cors,
Eigen::Vector3f  mo = Null3f(),
Eigen::Vector3f  mn = Null3f(),
Eigen::Vector3f  mot = Null3f() 
)

Definition at line 204 of file frir_freehand.cpp.

void visualize ( const pcl::PointCloud< pcl::PointXYZ > &  pc_old,
const pcl::PointCloud< pcl::PointXYZ > &  pc_new,
const std::vector< COR_S > &  cors,
const std::vector< int > &  weight_o,
const std::vector< int > &  weight_n 
)

Definition at line 342 of file frir_freehand.cpp.

void visualize ( const pcl::PointCloud< pcl::PointXYZRGB > &  pc_old)

Definition at line 417 of file frir_freehand.cpp.

float vorz ( float  v) [inline]

Definition at line 1213 of file frir_freehand.cpp.

Variable Documentation

bool g_step = false

Definition at line 80 of file frir_freehand.cpp.

std_msgs::Header header_

Definition at line 106 of file frir_freehand.cpp.

ros::Publisher map_pub_

Definition at line 108 of file frir_freehand.cpp.

ros::Publisher marker_pub_

Definition at line 107 of file frir_freehand.cpp.

ros::Publisher point_cloud_pub_

Definition at line 108 of file frir_freehand.cpp.


cob_3d_registration
Author(s): Joshua Hampp
autogenerated on Wed Aug 26 2015 11:02:36