introducing second corrector to corrector_test, making use of AddNoise

scripts/blackboxes.py

@@ -6,7 +6,7 @@
 dir = "/wg/stor2a/mkrainin/object_data/perception challenge/object_meshes"
 
 class InputsModule(ecto.BlackBox):
-    def __init__(self,plasm):
+    def __init__(self,plasm,rot_noise=0,trans_noise=0):
         ecto.BlackBox.__init__(self, plasm)
 
         #subscribers
@@ -19,6 +19,9 @@ def __init__(self,plasm):
 
         #object detection
         self.tod_detector = ecto_corrector.TODServiceCaller("TOD",ply_dir=dir)
+
+        #artificial noise
+        self.noise_adder = ecto_corrector.AddNoise("Noise Adder",rotation=rot_noise,translation=trans_noise)
 
         #edge detection
         self.edge_detector = ecto_corrector.DepthEdgeDetectorXYZ("Edge Detector",depth_threshold=0.02, \
@@ -36,7 +39,7 @@ def __init__(self,plasm):
 
     def expose_outputs(self):
         return {
-                 "pose":self.tod_detector["pose"],
+                 "pose":self.noise_adder["out_pose"],
                  "model":self.model_loader["model"],
                  "info":self.roi["out_camera_info"],
                  "depth_edges":self.edge_detector["depth_edges"],
@@ -56,8 +59,12 @@ def connections(self):
             #model loading
             self.tod_detector["ply_file"]>> self.model_loader[:],
 
+            #artificial noise
+            self.tod_detector["pose"]   >> self.noise_adder["in_pose"],
+            self.model_loader[:]        >> self.noise_adder["model"],
+
             #region of interest
-            self.tod_detector["pose"]    >> self.roi["pose"],
+            self.noise_adder["out_pose"] >> self.roi["pose"],
             self.model_loader[:]         >> self.roi["model"],
             self.sub_info[:]             >> self.roi["in_camera_info"],
 

scripts/corrector_test.py

@@ -6,8 +6,9 @@
 from blackboxes import *
 
 import sys
+import math
 
-debug = True
+debug = False
 
 if __name__ == "__main__":
     node_name = "corrector_test"
@@ -16,30 +17,43 @@
     plasm = ecto.Plasm()
 
     #inputs
-    inputs = InputsModule(plasm)
+    inputs = InputsModule(plasm,rot_noise=15*math.pi/180,trans_noise=0.05)
 
     #pose correction
-    corrector = ecto_corrector.CorrectorXYZ("Corrector")  #TODO: params
+    beam_corrector = ecto_corrector.CorrectorXYZ("Beam Corrector",window_half=2,sigma_pixel=5.0)
+    icp_corrector = ecto_corrector.CorrectorXYZ("ICP Corrector",use_icp=True,use_sensor_model=False)
 
     #display
     pre_correct_vertices = VerticesPubModule(plasm,node_name+"/pre_correct")
-    post_correct_vertices = VerticesPubModule(plasm,node_name+"/post_correct")
+    post_beam_vertices = VerticesPubModule(plasm,node_name+"/post_beam")
+    post_icp_vertices = VerticesPubModule(plasm,node_name+"/post_icp")
 
     graph = [
         #pre-correct visualization
         inputs["pose"]          >> pre_correct_vertices["pose"],
         inputs["model"]         >> pre_correct_vertices["model"],
 
-        #correction
-        inputs["pose"]          >> corrector["input_pose"],
-        inputs["model"]         >> corrector["model"],
-        inputs["info"]          >> corrector["camera_info"],
-        inputs["depth_edges"]   >> corrector["depth_edges"],
-        inputs["cloud"]         >> corrector["cloud"],         
+        #beam correction
+        inputs["pose"]          >> beam_corrector["input_pose"],
+        inputs["model"]         >> beam_corrector["model"],
+        inputs["info"]          >> beam_corrector["camera_info"],
+        inputs["depth_edges"]   >> beam_corrector["depth_edges"],
+        inputs["cloud"]         >> beam_corrector["cloud"],         
 
-        #pose-correct visualization
-        corrector["output_pose"]>> post_correct_vertices["pose"],
-        inputs["model"]         >> post_correct_vertices["model"],
+        #beam-correct visualization
+        beam_corrector["output_pose"]>> post_beam_vertices["pose"],
+        inputs["model"]              >> post_beam_vertices["model"],
+
+        #icp correction
+        inputs["pose"]          >> icp_corrector["input_pose"],
+        inputs["model"]         >> icp_corrector["model"],
+        inputs["info"]          >> icp_corrector["camera_info"],
+        inputs["depth_edges"]   >> icp_corrector["depth_edges"],
+        inputs["cloud"]         >> icp_corrector["cloud"],  
+
+        #icp-correct visualization
+        icp_corrector["output_pose"]>> post_icp_vertices["pose"],
+        inputs["model"]             >> post_icp_vertices["model"],
     ]
 
     plasm.connect(graph)
@@ -50,7 +64,11 @@
     sched = ecto.schedulers.Threadpool(plasm)
 
     print "Press enter to run once. 'q' to quit"
-    while(raw_input() != 'q'):
+    result = 0
+    while(raw_input() != 'q' and result == 0):
         print "Running Once"
-        sched.execute(1)   
-        print "Done. Press enter to run again. 'q' to quit"
+        result = sched.execute(1)   
+        if(result == 0):
+            print "Done. Press enter to run again. 'q' to quit"
+        else:
+            print "Execution failed (probably no TOD detections)"

src/Corrector.cpp

@@ -88,10 +88,14 @@ namespace ecto_corrector
       tf::Transform init_pose;
       tf::poseMsgToTF(in_pose_->pose,init_pose);
 
+      //copy the model so there are no issues with others trying to set pose
+      boost::shared_ptr<pose_corrector::RigidObjectModel> model(new pose_corrector::RigidObjectModel());
+      *model = **model_;
+
       //set up and run corrector
       pose_corrector::Corrector corrector;
-      (*model_)->setBasePose(init_pose);
-      corrector.setModel(*model_);
+      model->setBasePose(init_pose);
+      corrector.setModel(model);
       corrector.initCamera(*cam_info_);
 
       pose_corrector::CorrectorParams params;
@@ -126,7 +130,7 @@ namespace ecto_corrector
 
       //set output pose
       out_pose_->header = in_pose_->header;
-      tf::poseTFToMsg((*model_)->getBasePose(),out_pose_->pose);
+      tf::poseTFToMsg(model->getBasePose(),out_pose_->pose);
 
       return ecto::OK;
     }