Skip to content

Commit

Permalink
added random restarts to Corrector
Browse files Browse the repository at this point in the history
  • Loading branch information
Mike Krainin committed Sep 20, 2011
1 parent 963036a commit c5cae3c
Show file tree
Hide file tree
Showing 2 changed files with 87 additions and 48 deletions.
89 changes: 57 additions & 32 deletions scripts/corrector_test.py
View file
Original file line number Diff line number Diff line change
Expand Up @@ -11,8 +11,12 @@
visualize = True
debug_graphs = True
dir = "/wg/stor2a/mkrainin/object_data/perception challenge/object_meshes"
#noise
rotation = 10*math.pi/180
translation = 0.03
#optimization params
use_icp = False #uses sensor model instead if false
restarts = 10

if __name__ == "__main__":
node_name = "corrector_test"
Expand All @@ -38,21 +42,19 @@
#artificial noise
noise_adder = ecto_corrector.AddNoise("Noise Adder",
rotation=rotation,translation=translation)

#edge detection
edge_detector = ecto_corrector.DepthEdgeDetector("Edge Detector",
depth_threshold=0.02, erode_size=3,open_size=3)

#model loading
model_loader = ecto_corrector.ModelLoader("Model Loader")

#region of interest
roi = ecto_corrector.ModelROI("ROI",expansion=50,binning=2)
roi = ecto_corrector.ModelROI("ROI",expansion=70,binning=2)
apply_roi = ecto_corrector.ApplyROI("Apply ROI")

#pose correction
# beam_corrector = ecto_corrector.Corrector("Beam Corrector",window_half=2,sigma_pixel=5.0)
icp_corrector = ecto_corrector.Corrector("ICP Corrector",use_icp=True,use_sensor_model=False)
corrector = ecto_corrector.Corrector("Corrector",
use_icp=use_icp,use_sensor_model=(not use_icp),
restarts=restarts,iterations=(8 if use_icp else 4),
inner_loops=8, window_half=1)

sub_graph = [
#conversion
Expand All @@ -67,44 +69,67 @@
model_loader[:] >> roi["model"],
sub_info[:] >> roi["in_camera_info"],
roi[:] >> apply_roi["info"],
msg2cloud[:] >> apply_roi["input"],

#edge detection
apply_roi[:] >> edge_detector[:],

#beam correction
# noise_adder["out_pose"] >> beam_corrector["input_pose"],
# model_loader["model"] >> beam_corrector["model"],
# roi["out_camera_info"] >> beam_corrector["camera_info"],
# edge_detector["depth_edges"]>> beam_corrector["depth_edges"],
# cloud2typed[:] >> beam_corrector["input"],
msg2cloud[:] >> apply_roi["input"],

#icp correction
noise_adder["out_pose"] >> icp_corrector["input_pose"],
model_loader["model"] >> icp_corrector["model"],
roi["out_camera_info"] >> icp_corrector["camera_info"],
edge_detector["depth_edges"]>> icp_corrector["depth_edges"],
cloud2typed[:] >> icp_corrector["input"],
noise_adder["out_pose"] >> corrector["input_pose"],
model_loader["model"] >> corrector["model"],
roi["out_camera_info"] >> corrector["camera_info"],
cloud2typed[:] >> corrector["input"],
]

if use_icp:
#edge detection for boundary correspondence removal
edge_detector = ecto_corrector.DepthEdgeDetector("Edge Detector",
depth_threshold=0.02, erode_size=3,open_size=3)
sub_graph += [
apply_roi[:] >> edge_detector[:],
edge_detector["depth_edges"]>> corrector["depth_edges"],
]

if restarts > 0:
#normals
normals = ecto_pcl.NormalEstimation("Normals", k_search=0, radius_search=0.006,
spatial_locator=ecto_pcl.KDTREE_ORGANIZED_INDEX)
sub_graph += [ apply_roi[:] >> normals[:] ]

#segmentation for SEGMENTATION_SENSOR_MODEL restart comparison
segmenter = ecto_corrector.Segmenter("Segmenter",pixel_step=2,
depth_threshold=0.0015,
normal_threshold=0.99,
curvature_threshold=10, #not using curvature threshold
max_depth = 1.5)

sub_graph += [ apply_roi[:] >> segmenter["input"],
normals[:] >> segmenter["normals"],
segmenter["valid_segments"] >> corrector["valid_segments"],
segmenter["invalid"] >> corrector["invalid"], ]

if visualize:
pre_correct_vertices = VerticesPubModule(sub_plasm,node_name+"/pre_correct")
# post_beam_vertices = VerticesPubModule(sub_plasm,node_name+"/post_beam")
post_icp_vertices = VerticesPubModule(sub_plasm,node_name+"/post_icp")
post_correct_vertices = VerticesPubModule(sub_plasm,node_name+"/post_correct")

sub_graph += [
#pre-correct visualization
noise_adder["out_pose"] >> pre_correct_vertices["pose"],
model_loader["model"] >> pre_correct_vertices["model"],

#beam-correct visualization
# beam_corrector["output_pose"]>> post_beam_vertices["pose"],
# model_loader["model"] >> post_beam_vertices["model"],

#icp-correct visualization
icp_corrector["output_pose"]>> post_icp_vertices["pose"],
model_loader["model"] >> post_icp_vertices["model"],
corrector["output_pose"] >> post_correct_vertices["pose"],
model_loader["model"] >> post_correct_vertices["model"],
]

if use_icp:
depth_drawer = ecto_opencv.highgui.imshow("Drawer",name="depth edges", waitKey=10)
sub_graph += [edge_detector[:] >> depth_drawer[:]]

if restarts > 0:
seg2mat = ecto_corrector.SegmentsToMat("Seg2Mat",min_size=10)
seg_drawer = ecto_opencv.highgui.imshow("Drawer",name="segments", waitKey=10)
sub_graph += [ segmenter["valid_segments"] >> seg2mat["segments"],
apply_roi[:] >> seg2mat["input"],
seg2mat[:] >> seg_drawer[:],
]

sub_plasm.connect(sub_graph)
if(debug_graphs):
Expand Down Expand Up @@ -137,7 +162,7 @@
main_graph = [
#display
sub_image[:] >> img2mat[:],
img2mat[:] >> show["input"],
img2mat[:] >> show[:],

#triggering
show["d_key"] >> (tod_detector_if["__test__"],trigger_and["in1"]),
Expand Down
46 changes: 30 additions & 16 deletions src/Corrector.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -23,17 +23,19 @@ namespace ecto_corrector
static void declare_params(tendrils& params)
{
//high level params
params.declare<int>("iterations","Outer loop iterations",20);
params.declare<int>("inner_loops","Inner loop iterations",20);
params.declare<int>("restarts","Number of random restarts to use",0);
params.declare<int>("iterations","Outer loop iterations",10);
params.declare<int>("inner_loops","Inner loop iterations",10);
params.declare<double>("restart_max_angle","Max angle (degrees) for random restarts",30.0);
params.declare<double>("restart_max_translation","Max translation for random restarts",0.05);

//icp
params.declare<bool>("use_icp","Whether to use ICP constraints",false);

//sensor model
params.declare<bool>("use_sensor_model","Whether to use sensor model constraints",true);
params.declare<bool>("recompute_uncertainties","Whether to recompute uncertainties for sensor model constraints",true);
params.declare<double>("sigma_z","Starting standard deviation for z difference",0.01);
params.declare<double>("sigma_pixel","Starting standard deviation for pixel difference",2.0);
params.declare<double>("sigma_z","Starting standard deviation for z difference",0.005);
params.declare<double>("sigma_pixel","Starting standard deviation for pixel difference",1.0);
params.declare<int>("window_half","Half window size for windowed sensor model (0 for non-windowed)",0);

}
Expand All @@ -51,6 +53,12 @@ namespace ecto_corrector
"depth_edges","Image representing depth discontinuities (sized to ROI)");
in.declare<pcl::PointCloud<pcl::PointXYZRGB>::ConstPtr >(
"input","Cloud to align to");
in.declare<std::vector<std::vector<cv::Point2i> > > ("valid_segments",
"Segments as vectors of pixel indices. Only needed if using restarts",
std::vector<std::vector<cv::Point2i> >());
in.declare<std::vector<cv::Point2i> >("invalid", "Pixel indices for "\
"pixels marked as invalid by segmentation. Only needed if using restarts",
std::vector<cv::Point2i>());

//outputs
out.declare<geometry_msgs::PoseStamped>(
Expand All @@ -65,16 +73,20 @@ namespace ecto_corrector
cam_info_ = in["camera_info"];
depth_edges_ = in["depth_edges"];
cloud_ = in["input"];
valid_segments_ = in["valid_segments"];
invalid_ = in["invalid"];

//outputs
out_pose_ = out["output_pose"];

//params
restarts_ = params["restarts"];
iterations_ = params["iterations"];
g2o_inner_loops_ = params["inner_loops"];
restart_max_degrees_ = params["restart_max_angle"];
restart_max_trans_ = params["restart_max_translation"];
use_icp_ = params["use_icp"];
use_sensor_model_ = params["use_sensor_model"];
recompute_uncertainties_ = params["recompute_uncertainties"];
sigma_z_ = params["sigma_z"];
sigma_pixel_ = params["sigma_pixel"];
window_half_ = params["window_half"];
Expand All @@ -83,8 +95,6 @@ namespace ecto_corrector

int process(const tendrils& in, const tendrils& out)
{
std::cout<<"In process"<<std::endl;

//get initial pose
tf::Transform init_pose;
tf::poseMsgToTF(in_pose_->pose,init_pose);
Expand All @@ -97,23 +107,25 @@ namespace ecto_corrector
pose_corrector::Corrector corrector;
model->setBasePose(init_pose);
corrector.setModel(model);
corrector.initCamera(*cam_info_,(*cloud_)->width);
corrector.initCamera(*cam_info_,640); //second arg should be width of pre-cropped cloud

pose_corrector::CorrectorParams params;
params.restarts = *restarts_;
params.restart_max_degrees = *restart_max_degrees_;
params.restart_max_translation = *restart_max_trans_;
params.restart_function = pose_corrector::SEGMENTATION_SENSOR_MODEL;
params.restart_sigma_z = *sigma_z_;
params.iterations = *iterations_;
params.optimizer_params.g2o_iterations = *g2o_inner_loops_;
params.optimizer_params.use_icp_constraints = *use_icp_;
params.optimizer_params.use_sensor_model_constraints = *use_sensor_model_;
params.optimizer_params.sensor_model_recompute_uncertainties = *recompute_uncertainties_;
params.optimizer_params.sensor_model_z_st_dev = *sigma_z_;
params.optimizer_params.sensor_model_pixel_st_dev = *sigma_pixel_;
params.optimizer_params.sensor_model_window_half = *window_half_;
corrector.setParams(params);

//perform the correction
corrector.correct(**cloud_,*depth_edges_,
std::vector<std::vector<cv::Point2i> >(), //TODO: take in segmentation
std::vector<cv::Point2i>());
corrector.correct(**cloud_,*depth_edges_,*valid_segments_,*invalid_);

//set output pose
out_pose_->header = in_pose_->header;
Expand All @@ -129,14 +141,16 @@ namespace ecto_corrector
ecto::spore<sensor_msgs::CameraInfoConstPtr> cam_info_;
ecto::spore<cv::Mat> depth_edges_;
ecto::spore<pcl::PointCloud<pcl::PointXYZRGB>::ConstPtr> cloud_;
ecto::spore<std::vector<std::vector<cv::Point2i> > > valid_segments_;
ecto::spore<std::vector<cv::Point2i> > invalid_;

//outputs
ecto::spore<geometry_msgs::PoseStamped> out_pose_;

//params
ecto::spore<int> iterations_, g2o_inner_loops_, window_half_;
ecto::spore<bool> use_icp_, use_sensor_model_, recompute_uncertainties_;
ecto::spore<double> sigma_z_, sigma_pixel_;
ecto::spore<int> restarts_, iterations_, g2o_inner_loops_, window_half_;
ecto::spore<bool> use_icp_, use_sensor_model_;
ecto::spore<double> sigma_z_, sigma_pixel_, restart_max_degrees_, restart_max_trans_;

};
} //namespace
Expand Down

0 comments on commit c5cae3c

Please sign in to comment.