def adjust_fold(self, req): #Go to viewing stance StanceUtils.call_stance("open_both", 5.0) StanceUtils.call_stance("viewing", 5.0) last_model = pickle.load(open("/tmp/last_model.pickle")) camera_model = image_geometry.PinholeCameraModel() info = RosUtils.get_next_message("wide_stereo/left/camera_info", CameraInfo) cam_frame = info.header.frame_id camera_model.fromCameraInfo(info) now = rospy.Time.now() req.start.header.stamp = now req.end.header.stamp = now self.listener.waitForTransform(cam_frame, req.start.header.frame_id, now, rospy.Duration(20.0)) start_cam = self.listener.transformPoint(cam_frame, req.start) end_cam = self.listener.transformPoint(cam_frame, req.end) start = camera_model.project3dToPixel( (start_cam.point.x, start_cam.point.y, start_cam.point.z)) end = camera_model.project3dToPixel( (end_cam.point.x, end_cam.point.y, end_cam.point.z)) folded_model = Models.Point_Model_Folded(last_model, start, end) folded_model.image = None folded_model.initial_model.image = None pickle.dump(folded_model, open("/tmp/last_model.pickle", 'w')) adjust_folded_model = rospy.ServiceProxy( "fold_finder_node/process_mono", ProcessMono) resp = adjust_folded_model("wide_stereo/left") new_start = resp.pts3d[0] new_end = resp.pts3d[1] return AdjustFoldResponse(start=new_start, end=new_end)
def getModelSymm(self): poly = self.getPolys()[0].getShape() #Due to symmetry, we only need half the points) vertices = poly.vertices()[0:len(poly.vertices()) / 2] tuple_vertices = [v.toTuple() for v in vertices] model = Models.Model_Symm(tuple_vertices, self.symmline) return model
def getModelPantsSkel(self): #Parameters: mid_center,top_center,mid_left,left_leg_center,left_leg_left width = Geometry2D.distance(self.left_leg_left, self.left_leg_right) return Models.Model_Pants_Skel_New(True, self.mid_center.toTuple(), self.top_center.toTuple(), self.mid_left.toTuple(), self.left_leg_center.toTuple(), self.top_left.toTuple(), width)
def getModelSkel(self): left_sleeve_width = Geometry2D.distance(self.sleeve_top, self.sleeve_node) * 2 return Models.Model_Shirt_Skel_Less_Restricted( True, self.spine_bottom.toTuple(), self.spine_top.toTuple(), self.collar.toTuple(), self.shoulder_joint.toTuple(), self.shoulder_top.toTuple(), self.sleeve_node.toTuple(), self.bottom_left.toTuple(), left_sleeve_width)
def getModelTee(self): left_sleeve_width = Geometry2D.distance(self.sleeve_top, self.sleeve_node) * 2 return Models.Model_Tee_Skel_No_Skew(True, self.spine_bottom.toTuple(), self.spine_top.toTuple(), self.collar.toTuple(), self.shoulder_joint.toTuple(), self.shoulder_top.toTuple(), self.sleeve_node.toTuple(), self.bottom_left.toTuple(), left_sleeve_width)
def saveModel(self): file = open(self.output_modelpath,'w') displacement = Vector2D.intercept(self.foldline,Vector2D.horiz_ln(y=0))[0] (dx,dy) = Vector2D.line_vector(self.foldline) angle = abs(arctan(dy / dx)) if dx > 0 and dy > 0: angle = angle elif dx < 0 and dy > 0: angle = pi - angle elif dx < 0 and dy < 0: angle = pi + angle else: angle *= -1 model = Models.Point_Model_Folded(self.initial_model,self.foldline_pts[0],self.foldline_pts[1]) model.draw_to_image(self.background,cv.RGB(255,0,0)) if model.illegal() or model.structural_penalty() >= 1.0: print "Model is illegal!" self.clearAll() else: model.set_image(None) pickle.dump(model,file)
def process(self,cv_image,info,image2=None): self.load_model(self.modelpath) if self.transform: H = cv.Load(self.matrix_location) input_image = cv.CloneImage(cv_image) cv.WarpPerspective(input_image,cv_image,H, cv.CV_INTER_LINEAR+cv.CV_WARP_INVERSE_MAP+cv.CV_WARP_FILL_OUTLIERS) #Use the thresholding module to get the contour out shape_contour = thresholding.get_contour(cv_image,bg_mode=self.bg_mode,filter_pr2=self.filter_pr2 ,crop_rect=(self.cropx,self.cropy,self.cropwidth,self.cropheight),cam_info=info,listener=self.listener) #Use the shape_fitting module to fit the model to the contour if self.mode=="tee": #fitter = shape_fitting.ShapeFitter(SYMM_OPT=True,ORIENT_OPT=False,FINE_TUNE=False) fitter = shape_fitting.ShapeFitter(SYMM_OPT=False,ORIENT_OPT=True,FINE_TUNE=False,num_iters=self.num_iters) elif self.mode=="sweater": fitter = shape_fitting.ShapeFitter(SYMM_OPT=False,ORIENT_OPT=False,FINE_TUNE=False,num_iters=self.num_iters) elif self.mode=="folded": fitter = shape_fitting.ShapeFitter(SYMM_OPT=False,ORIENT_OPT=False,FINE_TUNE=False,INITIALIZE=False,num_iters=self.num_iters) else: fitter = shape_fitting.ShapeFitter(SYMM_OPT=False,ORIENT_OPT=False,FINE_TUNE=False,num_iters=self.num_iters) image_anno = cv.CloneImage(cv_image) (nearest_pts, final_model, fitted_model) = fitter.fit(self.model,shape_contour,image_anno) pts = nearest_pts params = {} if self.mode == "triangles": return_pts = [pts[1],pts[4],pts[2],pts[3]] self.highlight_pt(pts[1],cv.CV_RGB(255,0,0),image_anno) font = cv.InitFont(cv.CV_FONT_HERSHEY_COMPLEX,1.0,1.0) cv.PutText(image_anno,"(l)eft",(pts[1][0]-20,pts[1][1]-15),font,cv.CV_RGB(255,0,0)) self.highlight_pt(pts[4],cv.CV_RGB(0,0,255),image_anno) cv.PutText(image_anno,"(r)ight",(pts[4][0]-20,pts[4][1]-15),font,cv.CV_RGB(0,0,255)) params = {"tilt":0.0} elif self.mode == "towel": return_pts = pts elif self.mode == "tee" or self.mode == "sweater": return_pts = pts[0:5]+pts[8:] params = {} elif self.mode == "folded": return_pts = [final_model.fold_bottom(),final_model.fold_top()] else: return_pts = pts params = {} if self.transform: H_inv = cv.CloneMat(H) cv.Invert(H,H_inv) anno_unchanged = cv.CloneImage(image_anno) cv.WarpPerspective(anno_unchanged,image_anno,H_inv, cv.CV_INTER_LINEAR+cv.CV_WARP_INVERSE_MAP+cv.CV_WARP_FILL_OUTLIERS) new_return_pts = self.transform_pts(return_pts,H_inv) for i,pt in enumerate(new_return_pts): return_pts[i] = pt if self.mode != "triangles": for pt in return_pts: self.highlight_pt(pt,cv.CV_RGB(255,0,0),image_anno) if self.mode != "folded": fitted_model.set_image(None) pickle.dump(fitted_model,open("%s/last_model.pickle"%self.save_dir,'w')) else: model_pts = final_model.vertices_full() new_model = Models.Point_Model_Contour_Only_Asymm(*model_pts) pickle.dump(new_model,open("%s/last_model.pickle"%self.save_dir,'w')) score = final_model.score(shape_contour) #fitter.energy_fxn(final_model,shape_contour) params["score"] = score return (return_pts,params,image_anno)
def getModelSockSkel(self): return Models.Model_Sock_Skel(True, self.ankle_center.toTuple(), self.ankle_joint.toTuple(), self.toe_center.toTuple(), self.sock_width, self.sock_width / 2)
def getModelAsymm(self): poly = self.getPolys()[0].getShape() vertices = poly.vertices() tuple_vertices = tuple([v.toTuple() for v in vertices]) model = Models.Point_Model_Contour_Only_Asymm(*tuple_vertices) return model
def fit(self, model, contour, img_annotated=None, img=None): assert not model.illegal() if not img_annotated: xs = [x for (x, y) in contour] ys = [y for (x, y) in contour] width = max(xs) - min(xs) height = max(ys) - min(ys) cv.Set(img_annotated, cv.CV_RGB(255, 255, 255)) model.set_image(cv.CloneImage(img_annotated)) shape_contour = contour if SHOW_CONTOURS: cv.DrawContours(img_annotated, shape_contour, cv.CV_RGB(255, 0, 0), cv.CV_RGB(255, 0, 0), 0, 1, 8, (0, 0)) if self.INITIALIZE: self.printout("INITIALIZING") (real_center, real_top, real_theta, real_scale) = get_principle_info(shape_contour) if SHOW_UNSCALED_MODEL: model.draw_to_image(img_annotated, cv.CV_RGB(0, 0, 255)) model_contour = model.vertices_dense(constant_length=False, density=30) (model_center, model_top, model_theta, model_scale) = get_principle_info(model_contour) displ = displacement(model_center, real_center) #Drawing if SHOW_INIT_PTS: top_img = cv.CloneImage(img_annotated) cv.DrawContours(top_img, shape_contour, cv.CV_RGB(255, 0, 0), cv.CV_RGB(255, 0, 0), 0, 1, 8, (0, 0)) model.draw_contour(top_img, cv.CV_RGB(0, 0, 255), 2) draw_pt(top_img, real_top, cv.CV_RGB(255, 0, 0)) draw_pt(top_img, real_center, cv.CV_RGB(255, 0, 0)) draw_pt(top_img, model_top, cv.CV_RGB(0, 0, 255)) draw_pt(top_img, model_center, cv.CV_RGB(0, 0, 255)) cv.NamedWindow("Top") cv.ShowImage("Top", top_img) cv.WaitKey() angle = model_theta - real_theta if self.ORIENT_OPT: angle = 0 scale = real_scale / float(model_scale) if scale < 0.25: scale = 1 model_trans = translate_poly(model.polygon_vertices(), displ) model_rot = rotate_poly(model_trans, -1 * angle, real_center) model_scaled = scale_poly(model_rot, scale, real_center) #(model_center,model_top,model_theta,model_scale) = get_principle_info(model_scaled) #Do the same to the actual model model.translate(displ) if self.ROTATE: model.rotate(-1 * angle, real_center) model.scale(scale, real_center) if SHOW_SCALED_MODEL: model.draw_to_image(img_annotated, cv.CV_RGB(0, 0, 255)) self.printout("Energy is: %f" % model.score(shape_contour)) self.printout("Shape contour has %d points" % (len(shape_contour))) sparse_shape_contour = make_sparse(shape_contour, 1000) #Optimize if self.ORIENT_OPT: init_model = Models.Orient_Model(model, pi / 2) orient_model_finished = self.black_box_opt( model=init_model, contour=shape_contour, num_iters=self.num_iters, delta=init_model.preferred_delta(), epsilon=0.01, mode="orient", image=img) model_oriented = orient_model_finished.transformed_model() else: model_oriented = model if self.SYMM_OPT: self.printout("SYMMETRIC OPTIMIZATION") new_model_symm = self.black_box_opt(model=model_oriented, contour=shape_contour, num_iters=self.num_iters, delta=model.preferred_delta(), epsilon=0.01, mode="symm", image=img) else: new_model_symm = model_oriented if SHOW_SYMM_MODEL: new_model_symm.draw_to_image(img=img_annotated, color=cv.CV_RGB(0, 255, 0)) model = new_model_symm.make_asymm() if self.HIGH_EXPLORATION: exp_factor = 3.0 else: exp_factor = 1.5 if self.ASYMM_OPT: new_model_asymm = self.black_box_opt(model=model, contour=shape_contour, num_iters=self.num_iters, delta=model.preferred_delta(), exploration_factor=exp_factor, fine_tune=False, mode="asymm", image=img) else: new_model_asymm = model if self.FINE_TUNE: #tunable_model = model_oriented.make_tunable() tunable_model = new_model_asymm.make_tunable() final_model = self.black_box_opt(model=tunable_model, contour=shape_contour, num_iters=self.num_iters, delta=5.0, exploration_factor=1.5, fine_tune=False, image=img) final_model = final_model.final() else: final_model = new_model_asymm final_model.draw_to_image(img=img_annotated, color=cv.CV_RGB(255, 0, 255)) nearest_pts = [] for vert in final_model.polygon_vertices(): nearest_pt = min(shape_contour, key=lambda pt: Vector2D.pt_distance(pt, vert)) cv.Circle(img_annotated, nearest_pt, 5, cv.CV_RGB(255, 255, 255), 3) nearest_pts.append(nearest_pt) fitted_model = Models.Point_Model_Contour_Only_Asymm(*nearest_pts) #fitted_model = final_model if SHOW_FITTED: fitted_model.draw_to_image(img=img_annotated, color=cv.CV_RGB(0, 255, 255)) return (nearest_pts, final_model, fitted_model)