00001 /* 00002 * icp_utils.h 00003 * 00004 * Created on: Feb 15, 2010 00005 * Author: sturm 00006 */ 00007 00008 #ifndef ICP_UTILS_H_ 00009 #define ICP_UTILS_H_ 00010 00011 #include "articulation_msgs/TrackMsg.h" 00012 #include "icp/icpCpp.h" 00013 #include "icp/kdtree_common.h" 00014 00015 namespace icp { 00016 00017 class TrajData { 00018 public: 00019 size_t n; 00020 double *points; 00021 double *pointsT; 00022 double *weights; 00023 int *index; 00024 unsigned int *randvec; 00025 TrajData(const articulation_msgs::TrackMsg &track); 00026 ~TrajData(); 00027 }; 00028 00029 class IcpAlign { 00030 public: 00031 TrajData model; 00032 TrajData data; 00033 int iter; 00034 icp::Tree *tree; 00035 double TR[9]; 00036 double TT[3]; 00037 public: 00038 IcpAlign(const articulation_msgs::TrackMsg &track_model,const articulation_msgs::TrackMsg &track_data, int iter=40); 00039 ~IcpAlign(); 00040 void TransformData(articulation_msgs::TrackMsg &track_data_aligned); 00041 void TransformModel(articulation_msgs::TrackMsg &track_model_aligned); 00042 00043 }; 00044 00045 } 00046 00047 #endif /* ICP_UTILS_H_ */
