Skip to content

Commit

Permalink
Refactor codes
Browse files Browse the repository at this point in the history
  • Loading branch information
@koh-murakami-ai
koh-murakami-ai committed May 9, 2018
1 parent d065944 commit 4ab2700
Show file tree
Hide file tree
Showing 9 changed files with 259 additions and 239 deletions.
31 changes: 16 additions & 15 deletions ...c/computing/perception/detection/packages/lidar_tracker/launch/imm_ukf_pda_tracker.launch
View file
Original file line number Diff line number Diff line change
@@ -1,19 +1,20 @@
<!-- -->
<launch>
<!-- <arg name="init_" default="false" /> -->
<arg name="gating_thres_" default="9.22" />
<arg name="gate_probability_" default="0.99" />
<arg name="detection_probability_" default="0.9" />
<arg name="distance_thres_" default="99" />
<arg name="life_time_thres_" default="8" />
<!-- <arg name="bb_yaw_change_thres_" default="0.3" /> -->
<arg name="static_distance_thres_" default="3.0" />
<arg name="input_topic_" default="/cloud_clusters" />
<arg name="output_topic_" default="/tracking_cluster_array" />
<!-- <arg name="init_" default="false" /> -->
<arg name="gating_thres_" default="9.22" />
<arg name="gate_probability_" default="0.99" />
<arg name="detection_probability_" default="0.9" />
<arg name="distance_thres_" default="99" />
<arg name="life_time_thres_" default="8" />
<!-- <arg name="bb_yaw_change_thres_" default="0.3" /> -->
<arg name="static_distance_thres_" default="3.0" />
<arg name="input_topic_" default="/cloud_clusters" />
<arg name="output_topic_" default="/tracking_cluster_array" />
<arg name="pointcloud_frame_" default="/velodyne" />

<!-- rosrun lidar_tracker euclidean_cluster _points_node:="" -->
<node pkg="lidar_tracker" type="imm_ukf_pda_tracker" name="imm_ukf_pda_tracker" output="screen">
<!-- <param name="init_" value="$(arg init_)" /> -->
<!-- rosrun lidar_tracker euclidean_cluster _points_node:="" -->
<node pkg="lidar_tracker" type="imm_ukf_pda_tracker" name="imm_ukf_pda_tracker" output="screen">
<!-- <param name="init_" value="$(arg init_)" /> -->
<param name="gating_thres_" value="$(arg gating_thres_)" />
<param name="gate_probability_" value="$(arg gate_probability_)" />
<param name="detection_probability_" value="$(arg detection_probability_)" />
Expand All @@ -23,6 +24,6 @@
<param name="static_distance_thres_" value="$(arg static_distance_thres_)" />
<param name="input_topic_" value="$(arg input_topic_)" />
<param name="output_topic_" value="$(arg output_topic_)" />

</node>
<param name="pointcloud_frame_" value="$(arg pointcloud_frame_)" />
</node>
</launch>
12 changes: 7 additions & 5 deletions ...mputing/perception/detection/packages/lidar_tracker/launch/visualize_cloud_cluster.launch
View file
Original file line number Diff line number Diff line change
@@ -1,9 +1,11 @@
<!-- -->
<launch>
<arg name="input_topic_" default="/tracking_cluster_array" />
<!-- rosrun lidar_tracker euclidean_cluster _points_node:="" -->
<node pkg="lidar_tracker" type="visualize_cloud_cluster" name="visualize_cloud_cluster" output="screen">
<param name="input_topic_" value="$(arg input_topic_)" />
</node>
<arg name="input_topic_" default="/tracking_cluster_array" />
<arg name="pointcloud_frame_" default="/velodyne" />
<!-- rosrun lidar_tracker euclidean_cluster _points_node:="" -->
<node pkg="lidar_tracker" type="visualize_cloud_cluster" name="visualize_cloud_cluster" output="screen">
<param name="input_topic_" value="$(arg input_topic_)" />
<param name="pointcloud_frame_" value="$(arg pointcloud_frame_)" />
</node>

</launch>
134 changes: 66 additions & 68 deletions .../detection/packages/lidar_tracker/nodes/imm_ukf_pda_tracker/include/imm_ukf_pda_tracker.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -25,92 +25,90 @@
class ImmUkfPda
{
private:
bool init_;
double timestamp_ ;
// double ego_velo_;
// double egoYaw_;
// double egoPreYaw_;
// Eigen::VectorXd preMeas_;
std::vector<UKF> targets_;
// std::vector<int> trackNumVec_;

// probabilistic data association params
double gating_thres_;//9.22; // 99%
double gate_probability_;//0.99;
// extern double gammaG_ = 5.99; // 99%
// extern double pG_ = 0.95;
// extern double gammaG_ = 15.22; // 99%
double detection_probability_;//0.9;

//bbox association param
double distance_thres_;//0.25;
int life_time_thres_;//8;
//bbox update params
double bb_yaw_change_thres_;//0.2;
// double bb_area_change_thres_;//0.5;

double static_distance_thres_;

double init_yaw_;

std::string input_topic_;
std::string output_topic_;

// std::vector<UKF> targets_;
// std::vector<int> trackNumVec_;

tf::TransformListener* tran_;

ros::NodeHandle node_handle_;
bool init_;
double timestamp_ ;

std::vector<UKF> targets_;
// std::vector<int> trackNumVec_;

// probabilistic data association params
double gating_thres_;//9.22; // 99%
double gate_probability_;//0.99;
// extern double gammaG_ = 5.99; // 99%
// extern double pG_ = 0.95;
// extern double gammaG_ = 15.22; // 99%
double detection_probability_;//0.9;

//bbox association param
double distance_thres_;//0.25;
int life_time_thres_;//8;
//bbox update params
double bb_yaw_change_thres_;//0.2;
// double bb_area_change_thres_;//0.5;

double static_distance_thres_;

double init_yaw_;

std::string input_topic_;
std::string output_topic_;

std::string pointcloud_frame_;
// std::vector<UKF> targets_;
// std::vector<int> trackNumVec_;

tf::TransformListener* tran_;

ros::NodeHandle node_handle_;
ros::Subscriber sub_cloud_array_;
ros::Publisher pub_cloud_array_;

void callBack(autoware_msgs::CloudClusterArray input);
void transformPoseToGlobal(autoware_msgs::CloudClusterArray& input);
void transformPoseToLocal(autoware_msgs::CloudClusterArray& input);
void findMaxZandS(const UKF target, Eigen::VectorXd& max_det_z, Eigen::MatrixXd& max_det_s);
void measurementValidation(const autoware_msgs::CloudClusterArray input, UKF& target, const bool second_init,
const Eigen::VectorXd max_det_z, const Eigen::MatrixXd max_det_s,
std::vector<autoware_msgs::CloudCluster>& cluster_vec,
std::vector<int>& matching_vec);
void filterPDA(UKF& target, const std::vector<autoware_msgs::CloudCluster> cluster_vec, std::vector<double>& lambda_vec);
void getNearestEuclidCluster(const UKF target, const std::vector<autoware_msgs::CloudCluster> cluster_vec,
autoware_msgs::CloudCluster& cluster, double& min_dist);
void getRightAngleBBox(const std::vector<double> nearest_bbox, std::vector<double>& rightAngle_bbox);
void associateBB(const std::vector<autoware_msgs::CloudCluster> cluster_vec,
void findMaxZandS(const UKF target, Eigen::VectorXd& max_det_z, Eigen::MatrixXd& max_det_s);
void measurementValidation(const autoware_msgs::CloudClusterArray input, UKF& target, const bool second_init,
const Eigen::VectorXd max_det_z, const Eigen::MatrixXd max_det_s,
std::vector<autoware_msgs::CloudCluster>& cluster_vec,
std::vector<int>& matching_vec);
void filterPDA(UKF& target, const std::vector<autoware_msgs::CloudCluster> cluster_vec, std::vector<double>& lambda_vec);
void getNearestEuclidCluster(const UKF target, const std::vector<autoware_msgs::CloudCluster> cluster_vec,
autoware_msgs::CloudCluster& cluster, double& min_dist);
void getRightAngleBBox(const std::vector<double> nearest_bbox, std::vector<double>& rightAngle_bbox);
void associateBB(const std::vector<autoware_msgs::CloudCluster> cluster_vec,
UKF& target);
double getBboxArea(const pcl::PointCloud<pcl::PointXYZ> bbox);
double getBBoxYaw(const UKF target);
double getJskBBoxArea(const jsk_recognition_msgs::BoundingBox jsk_bb);
double getJskBBoxYaw(const jsk_recognition_msgs::BoundingBox jsk_bb);
void updateBB(UKF& target);
double getIntersectCoef(const double vec1x, const double vec1y, const double vec2x, const double vec2y,
const double p_x, const double p_y, const double cp_x, const double cp_y);
void mergeOverSegmentation(const std::vector<UKF> targets);
double getBboxArea(const pcl::PointCloud<pcl::PointXYZ> bbox);
double getBBoxYaw(const UKF target);
double getJskBBoxArea(const jsk_recognition_msgs::BoundingBox jsk_bb);
double getJskBBoxYaw(const jsk_recognition_msgs::BoundingBox jsk_bb);
void updateBB(UKF& target);
double getIntersectCoef(const double vec1x, const double vec1y, const double vec2x, const double vec2y,
const double p_x, const double p_y, const double cp_x, const double cp_y);
void mergeOverSegmentation(const std::vector<UKF> targets);

void updateLabel(UKF target, autoware_msgs::CloudCluster& cc);
void updateLabel(UKF target, autoware_msgs::CloudCluster& cc);

void initTracker(autoware_msgs::CloudClusterArray input, double timestamp);
void secondInit(double dt, std::vector<autoware_msgs::CloudCluster> clusterVec, UKF &target);
void initTracker(autoware_msgs::CloudClusterArray input, double timestamp);
void secondInit(double dt, std::vector<autoware_msgs::CloudCluster> clusterVec, UKF &target);

void updateTrackingNum(std::vector<autoware_msgs::CloudCluster> cluster_vec, UKF& target);
void updateTrackingNum(std::vector<autoware_msgs::CloudCluster> cluster_vec, UKF& target);

void probabilisticDataAssociation(autoware_msgs::CloudClusterArray input,
double dt, double det_explode_param, std::vector<int>& matching_vec,
std::vector<double>& lambda_vec, UKF& target, bool& is_skip_target);
void makeNewTargets(double timestamp, autoware_msgs::CloudClusterArray input, std::vector<int> matching_vec);
void probabilisticDataAssociation(autoware_msgs::CloudClusterArray input,
double dt, double det_explode_param, std::vector<int>& matching_vec,
std::vector<double>& lambda_vec, UKF& target, bool& is_skip_target);
void makeNewTargets(double timestamp, autoware_msgs::CloudClusterArray input, std::vector<int> matching_vec);

void staticClassification();
void staticClassification();

void makeOutput(autoware_msgs::CloudClusterArray input,
autoware_msgs::CloudClusterArray& output);
void makeOutput(autoware_msgs::CloudClusterArray input,
autoware_msgs::CloudClusterArray& output);

void tracker(autoware_msgs::CloudClusterArray input,
void tracker(autoware_msgs::CloudClusterArray input,
autoware_msgs::CloudClusterArray& output);


public:
ImmUkfPda();
ImmUkfPda();
};

#endif /* OBJECT_TRACKING_IMM_UKF_JPDAF_H */
18 changes: 10 additions & 8 deletions ...on/detection/packages/lidar_tracker/nodes/imm_ukf_pda_tracker/src/imm_ukf_pda_tracker.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -35,9 +35,10 @@ int g_count = 0;
ImmUkfPda::ImmUkfPda()
{
ros::NodeHandle private_nh_("~");
private_nh_.param<int>("life_time_thres_", life_time_thres_, 8);
private_nh_.param<std::string>("input_topic_", input_topic_, "/cloud_clusters");
private_nh_.param<std::string>("output_topic_", output_topic_, "/tracking_cluster_array");
private_nh_.param<std::string>("pointcloud_frame_", pointcloud_frame_, "velodyne");
private_nh_.param<int>("life_time_thres_", life_time_thres_, 8);
private_nh_.param<double>("gating_thres_", gating_thres_, 9.22);
private_nh_.param<double>("gate_probability_", gate_probability_, 0.99);
private_nh_.param<double>("detection_probability_", detection_probability_, 0.9);
Expand All @@ -59,7 +60,6 @@ ImmUkfPda::ImmUkfPda()
void ImmUkfPda::callBack(autoware_msgs::CloudClusterArray input)
{
autoware_msgs::CloudClusterArray output;

// only transform pose(clusteArray.clusters.bouding_box.pose)
transformPoseToGlobal(input);
tracker(input, output);
Expand All @@ -68,25 +68,27 @@ void ImmUkfPda::callBack(autoware_msgs::CloudClusterArray input)
pub_cloud_array_.publish(output);
g_count ++;
// std::cout << "Frame " <<g_count<< "------------------------------------------------"<<std::endl;
// std::cout << "target size "<<targets_.size() << std::endl;
}

void ImmUkfPda::transformPoseToGlobal(autoware_msgs::CloudClusterArray& input)
{
// std::string local_frame = "base_link";

for(size_t i = 0; i < input.clusters.size(); i ++)
{
geometry_msgs::PoseStamped pose_in, pose_out;

pose_in.header = input.header;
pose_in.pose = input.clusters[i].bounding_box.pose;
tran_->waitForTransform("/velodyne", "/world",input.header.stamp, ros::Duration(3.0));
tran_->waitForTransform(pointcloud_frame_, "/world",input.header.stamp, ros::Duration(3.0));
tran_->transformPose("world", ros::Time(0), pose_in, input.header.frame_id, pose_out);
input.clusters[i].bounding_box.pose = pose_out.pose;
}
}

void ImmUkfPda::transformPoseToLocal(autoware_msgs::CloudClusterArray& output)
{
// std::string local_frame = "base_link";
for(size_t i = 0; i < output.clusters.size(); i ++)
{
geometry_msgs::PoseStamped pose_in, pose_out;
Expand All @@ -95,9 +97,9 @@ void ImmUkfPda::transformPoseToLocal(autoware_msgs::CloudClusterArray& output)
pose_in.header.frame_id = "world";
pose_in.pose = output.clusters[i].bounding_box.pose;

tran_->waitForTransform("/world", "/velodyne",ros::Time(0), ros::Duration(3.0));
tran_->transformPose("velodyne", ros::Time(0), pose_in, "world", pose_out);
pose_out.header.frame_id = output.header.frame_id = "velodyne";
tran_->waitForTransform("/world", pointcloud_frame_, ros::Time(0), ros::Duration(3.0));
tran_->transformPose(pointcloud_frame_, ros::Time(0), pose_in, "world", pose_out);
pose_out.header.frame_id = output.header.frame_id = pointcloud_frame_;
output.clusters[i].bounding_box.pose = pose_out.pose;
}
}
Expand Down Expand Up @@ -752,7 +754,7 @@ void ImmUkfPda::makeOutput(autoware_msgs::CloudClusterArray input,
autoware_msgs::CloudClusterArray& output)
{
tf::StampedTransform transform;
tran_->lookupTransform("/world", "/velodyne", ros::Time(0), transform);
tran_->lookupTransform("/world", "/base_link", ros::Time(0), transform);

// get yaw angle from "world" to "velodyne" for direction(arrow) visualization
tf::Matrix3x3 m(transform.getRotation());
Expand Down
10 changes: 5 additions & 5 deletions ...tection/packages/lidar_tracker/nodes/imm_ukf_pda_tracker/src/imm_ukf_pda_tracker_main.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -9,10 +9,10 @@
int main(int argc, char **argv)
{

// std::cout<< 123<<std::endl;
ros::init(argc, argv, "imm_ukf_pda_tracker");
ImmUkfPda app;
ros::spin();
// std::cout<< 123<<std::endl;
ros::init(argc, argv, "imm_ukf_pda_tracker");
ImmUkfPda app;
ros::spin();

return 0;
return 0;
}
17 changes: 9 additions & 8 deletions ...on/packages/lidar_tracker/nodes/visualize_cloud_cluster/include/visualize_cloud_cluster.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -23,21 +23,22 @@
class VisualizeCloudCluster
{
private:
std::string input_topic_;
std::string input_topic_;
std::string pointcloud_frame_;

ros::NodeHandle node_handle_;
ros::NodeHandle node_handle_;
ros::Subscriber sub_cloud_array_;
ros::Publisher pub_jsk_bb_;
ros::Publisher pub_arrow_;
ros::Publisher pub_id_;
ros::Publisher pub_id_;

void getJskBBs(autoware_msgs::CloudClusterArray input,
jsk_recognition_msgs::BoundingBoxArray& jskBBs);
void visMarkers(autoware_msgs::CloudClusterArray input);
void callBack(autoware_msgs::CloudClusterArray input);
void getJskBBs(autoware_msgs::CloudClusterArray input,
jsk_recognition_msgs::BoundingBoxArray& jskBBs);
void visMarkers(autoware_msgs::CloudClusterArray input);
void callBack(autoware_msgs::CloudClusterArray input);

public:
VisualizeCloudCluster();
VisualizeCloudCluster();
};

#endif //OBJECT_TRACKING_VisualizeCloudCluster_H
Loading

0 comments on commit 4ab2700

Please sign in to comment.