00001
00002 import roslib
00003 roslib.load_manifest('stereo_anaglyph')
00004 import rospy
00005 import hrl_camera.ros_camera as rc
00006 import cv
00007
00008 def add_alpha_channel(bgr, alpha_val):
00009 w, h = cv.GetSize(bgr)
00010 bgra = cv.CreateImage((w, h), cv.IPL_DEPTH_8U, 4)
00011 alpha = cv.CreateImage((w, h), cv.IPL_DEPTH_8U, 1)
00012 chan1 = cv.CreateImage((w, h), cv.IPL_DEPTH_8U, 1)
00013 chan2 = cv.CreateImage((w, h), cv.IPL_DEPTH_8U, 1)
00014 chan3 = cv.CreateImage((w, h), cv.IPL_DEPTH_8U, 1)
00015 [cv.Set(c, 0) for c in [chan1, chan2, chan3, bgra, alpha]]
00016
00017 cv.Split(bgr, chan1, chan2, chan3, None)
00018 cv.Set(alpha, (alpha_val))
00019 cv.Merge(chan1, chan2, chan3, alpha, bgra)
00020 return bgra
00021
00022
00023 def remove_channels(in_bgra, channel_indices):
00024 w, h = cv.GetSize(in_bgra)
00025 chan1 = cv.CreateImage((w,h), cv.IPL_DEPTH_8U, 1)
00026 chan2 = cv.CreateImage((w,h), cv.IPL_DEPTH_8U, 1)
00027 chan3 = cv.CreateImage((w,h), cv.IPL_DEPTH_8U, 1)
00028 chan4 = cv.CreateImage((w,h), cv.IPL_DEPTH_8U, 1)
00029 bgra = cv.CreateImage((w,h), cv.IPL_DEPTH_8U, 4)
00030 [cv.Set(c, 0) for c in [chan1, chan2, chan3, chan4, bgra]]
00031 cv.Split(in_bgra, chan1, chan2, chan3, chan4)
00032 chan_list = [chan1, chan2, chan3, chan4]
00033 for i in channel_indices:
00034 chan_list[i] = None
00035 chan_list.append(bgra)
00036 cv.Merge(*tuple(chan_list))
00037 return bgra
00038
00039
00040 def anaglyph(left_color, right_color, correction):
00041
00042
00043 w, h = cv.GetSize(left_color)
00044 bgra = cv.CreateImage((w*2, h), cv.IPL_DEPTH_8U, 4)
00045 cv.Set(bgra, 0)
00046 right_bgra = add_alpha_channel(right_color, round(255/2.))
00047 left_bgra = add_alpha_channel(left_color, round(255/2.))
00048
00049
00050 left_red = remove_channels(left_bgra, [0, 1])
00051
00052 right_cyan = remove_channels(right_bgra, [2])
00053
00054
00055 if correction < 0:
00056 left_area = cv.GetSubRect(bgra, (-correction,0,w,h))
00057 right_area = cv.GetSubRect(bgra, (0, 0, w, h))
00058 valid_area = cv.GetSubRect(bgra, (-correction, 0, w + correction, h))
00059 else:
00060
00061 left_area = cv.GetSubRect(bgra, (0,0,w,h))
00062 right_area = cv.GetSubRect(bgra, (correction, 0, w, h))
00063 valid_area = cv.GetSubRect(bgra, (correction, 0, w - correction, h))
00064
00065 cv.Add(left_red, left_area, left_area)
00066 cv.Add(right_cyan, right_area, right_area)
00067
00068
00069
00070
00071
00072 return valid_area
00073
00074 if __name__ == '__main__':
00075 import optparse
00076 import time
00077 from sensor_msgs.msg import Image
00078 from cv_bridge.cv_bridge import CvBridge, CvBridgeError
00079
00080 p = optparse.OptionParser()
00081 p.add_option('-c', action='store', default='/wide_stereo', type='string', dest='cam', help='which camera to listen to')
00082 p.add_option('-d', action='store', default=30, type='int', dest='dist', help='separation distance')
00083 p.add_option('-s', action='store_true', dest='headless', help='headless mode')
00084 opt, args = p.parse_args()
00085
00086 cameras = [opt.cam + '/left/image_rect_color',
00087 opt.cam + '/right/image_rect_color']
00088 stereo_listener = rc.ROSStereoListener(cameras)
00089 if not opt.headless:
00090
00091
00092 cv.NamedWindow('stereo-anaglyph', 0)
00093 cv.ResizeWindow('stereo-anaglyph', 640, 480)
00094 cv.WaitKey(10)
00095 else:
00096 bridge = CvBridge()
00097 image_pub = rospy.Publisher('stereo_anaglyph', Image)
00098
00099 anaglyph_cyan_image_distance_correction = rospy.get_param('anaglyph_dist', opt.dist)
00100
00101 left = 1113937
00102 right = 1113939
00103 escape = 1048603
00104 while not rospy.is_shutdown():
00105 l, r = stereo_listener.next()
00106 red_blue = anaglyph(l, r, anaglyph_cyan_image_distance_correction)
00107 if not opt.headless:
00108
00109
00110 cv.ShowImage('stereo-anaglyph', red_blue)
00111 k = cv.WaitKey(10)
00112 print k
00113 if k == escape:
00114 break
00115 if k == left:
00116 anaglyph_cyan_image_distance_correction = anaglyph_cyan_image_distance_correction - 1
00117 print anaglyph_cyan_image_distance_correction
00118 if k == right:
00119 anaglyph_cyan_image_distance_correction = anaglyph_cyan_image_distance_correction + 1
00120 print anaglyph_cyan_image_distance_correction
00121 else:
00122 rosimage = bridge.cv_to_imgmsg(red_blue, "bgra8")
00123 image_pub.publish(rosimage)
00124
00125
00126
00127
00128
00129
00130
00131
00132
00133
00134
00135
00136
00137
00138
00139
00140
00141
00142
00143
00144
00145
00146
00147
00148
00149
00150
00151
00152
00153
00154
00155
00156
00157
00158
00159
00160
00161
00162
00163
00164
00165
00166
00167
00168
00169
00170
00171
00172
00173
00174
00175
00176
00177
00178
00179
00180
00181
00182
00183
00184
00185
00186
00187
00188
00189
00190
00191
00192
00193
00194
00195
00196
00197
00198
00199
00200
00201
00202