Skip to content

HTTPS clone URL

Subversion checkout URL

You can clone with HTTPS or Subversion.

Download ZIP
Browse files

added random restarts to Corrector

  • Loading branch information...
commit c5cae3c4350f8c3d6ab8a58897016ffba2fa0d51 1 parent 963036a
Mike Krainin authored
Showing with 87 additions and 48 deletions.
  1. +57 −32 scripts/corrector_test.py
  2. +30 −16 src/Corrector.cpp
View
89 scripts/corrector_test.py
@@ -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"
@@ -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
@@ -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):
@@ -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"]),
View
46 src/Corrector.cpp
@@ -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);
}
@@ -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>(
@@ -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"];
@@ -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);
@@ -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;
@@ -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
Please sign in to comment.
Something went wrong with that request. Please try again.