V-LOAM源码解析(六)

转载请注明出处:本文转自zhch_pan的博客http://www.cnblogs.com/zhchp-blog/

 

本博客为本人之前做项目时做的源码阅读工作,po到网上希望帮助其他人更好的理解V-LOAM的工程实现,有些地方代码做了修改,可能和原工程有出入,但对于该工程的整体流程理解没有妨碍。

源码下载链接:https://github.com/Jinqiang/demo_lidar

 


 

节点:transformMaintenance

功能:视觉里程计信息帧率较高,BA优化帧率较低,将二者结合,利用BA优化的结果修正视觉里程计,可以得到帧率和精度都较高的里程计信息

 

  1 #include <math.h>
  2 #include <stdio.h>
  3 #include <stdlib.h>
  4 #include <ros/ros.h>
  5 
  6 #include <nav_msgs/Odometry.h>
  7 
  8 #include <tf/transform_datatypes.h>
  9 #include <tf/transform_listener.h>
 10 #include <tf/transform_broadcaster.h>
 11 
 12 const double PI = 3.1415926;
 13 const double rad2deg = 180 / PI;
 14 const double deg2rad = PI / 180;
 15 const double conv[36]={0.05, 0.0, 0.0,  0.0, 0.0, 0.0, 
 16                        0.0, 0.05, 0.0,  0.0, 0.0, 0.0, 
 17                0.0, 0.0, 0.05, 0.0, 0.0, 0.0, 
 18                0.0, 0.0, 0.0,  1.0, 0.0, 0.0, 
 19                    0.0, 0.0, 0.0,  0.0, 1.0, 0.0, 
 20                0.0, 0.0, 0.0,  0.0, 0.0, 1.0};
 21 
 22 double timeOdomBefBA;
 23 double timeOdomAftBA;
 24 
 25 double rollRec, pitchRec, yawRec;
 26 double txRec, tyRec, tzRec;
 27 
 28 double transformBefBA[6] = {0};
 29 double transformAftBA[6] = {0};
 30 
 31 ros::Publisher *voData2PubPointer = NULL;
 32 tf::TransformBroadcaster *tfBroadcaster2Pointer = NULL;
 33 nav_msgs::Odometry voData2;
 34 tf::StampedTransform voDataTrans2;
 35 
 36 void transformAssociateToBA()
 37 {
 38   double txDiff = txRec - transformBefBA[3];
 39   double tyDiff = tyRec - transformBefBA[4];
 40   double tzDiff = tzRec - transformBefBA[5];
 41 
 42   double x1 = cos(yawRec) * txDiff + sin(yawRec) * tzDiff;
 43   double y1 = tyDiff;
 44   double z1 = -sin(yawRec) * txDiff + cos(yawRec) * tzDiff;
 45 
 46   double x2 = x1;
 47   double y2 = cos(pitchRec) * y1 - sin(pitchRec) * z1;
 48   double z2 = sin(pitchRec) * y1 + cos(pitchRec) * z1;
 49 
 50   txDiff = cos(rollRec) * x2 + sin(rollRec) * y2;
 51   tyDiff = -sin(rollRec) * x2 + cos(rollRec) * y2;
 52   tzDiff = z2;
 53 
 54   double sbcx = sin(-pitchRec);
 55   double cbcx = cos(-pitchRec);
 56   double sbcy = sin(-yawRec);
 57   double cbcy = cos(-yawRec);
 58   double sbcz = sin(rollRec);
 59   double cbcz = cos(rollRec);
 60 
 61   double sblx = sin(-transformBefBA[0]);
 62   double cblx = cos(-transformBefBA[0]);
 63   double sbly = sin(-transformBefBA[1]);
 64   double cbly = cos(-transformBefBA[1]);
 65   double sblz = sin(transformBefBA[2]);
 66   double cblz = cos(transformBefBA[2]);
 67 
 68   double salx = sin(-transformAftBA[0]);
 69   double calx = cos(-transformAftBA[0]);
 70   double saly = sin(-transformAftBA[1]);
 71   double caly = cos(-transformAftBA[1]);
 72   double salz = sin(transformAftBA[2]);
 73   double calz = cos(transformAftBA[2]);
 74 
 75   double srx = -sbcx*(salx*sblx + calx*caly*cblx*cbly + calx*cblx*saly*sbly) 
 76              - cbcx*cbcz*(calx*saly*(cbly*sblz - cblz*sblx*sbly) 
 77              - calx*caly*(sbly*sblz + cbly*cblz*sblx) + cblx*cblz*salx) 
 78              - cbcx*sbcz*(calx*caly*(cblz*sbly - cbly*sblx*sblz) 
 79              - calx*saly*(cbly*cblz + sblx*sbly*sblz) + cblx*salx*sblz);
 80   pitchRec = asin(srx);
 81 
 82   double srycrx = (cbcy*sbcz - cbcz*sbcx*sbcy)*(calx*saly*(cbly*sblz - cblz*sblx*sbly) 
 83                 - calx*caly*(sbly*sblz + cbly*cblz*sblx) + cblx*cblz*salx) 
 84                 - (cbcy*cbcz + sbcx*sbcy*sbcz)*(calx*caly*(cblz*sbly - cbly*sblx*sblz) 
 85                 - calx*saly*(cbly*cblz + sblx*sbly*sblz) + cblx*salx*sblz) 
 86                 + cbcx*sbcy*(salx*sblx + calx*caly*cblx*cbly + calx*cblx*saly*sbly);
 87   double crycrx = (cbcz*sbcy - cbcy*sbcx*sbcz)*(calx*caly*(cblz*sbly - cbly*sblx*sblz) 
 88                 - calx*saly*(cbly*cblz + sblx*sbly*sblz) + cblx*salx*sblz) 
 89                 - (sbcy*sbcz + cbcy*cbcz*sbcx)*(calx*saly*(cbly*sblz - cblz*sblx*sbly) 
 90                 - calx*caly*(sbly*sblz + cbly*cblz*sblx) + cblx*cblz*salx) 
 91                 + cbcx*cbcy*(salx*sblx + calx*caly*cblx*cbly + calx*cblx*saly*sbly);
 92   yawRec = -atan2(srycrx / cos(-pitchRec), crycrx / cos(-pitchRec));
 93 
 94   double srzcrx = sbcx*(cblx*cbly*(calz*saly - caly*salx*salz) 
 95                 - cblx*sbly*(caly*calz + salx*saly*salz) + calx*salz*sblx) 
 96                 - cbcx*cbcz*((caly*calz + salx*saly*salz)*(cbly*sblz - cblz*sblx*sbly) 
 97                 + (calz*saly - caly*salx*salz)*(sbly*sblz + cbly*cblz*sblx) 
 98                 - calx*cblx*cblz*salz) + cbcx*sbcz*((caly*calz + salx*saly*salz)*(cbly*cblz 
 99                 + sblx*sbly*sblz) + (calz*saly - caly*salx*salz)*(cblz*sbly - cbly*sblx*sblz) 
100                 + calx*cblx*salz*sblz);
101   double crzcrx = sbcx*(cblx*sbly*(caly*salz - calz*salx*saly) 
102                 - cblx*cbly*(saly*salz + caly*calz*salx) + calx*calz*sblx) 
103                 + cbcx*cbcz*((saly*salz + caly*calz*salx)*(sbly*sblz + cbly*cblz*sblx) 
104                 + (caly*salz - calz*salx*saly)*(cbly*sblz - cblz*sblx*sbly) 
105                 + calx*calz*cblx*cblz) - cbcx*sbcz*((saly*salz + caly*calz*salx)*(cblz*sbly 
106                 - cbly*sblx*sblz) + (caly*salz - calz*salx*saly)*(cbly*cblz + sblx*sbly*sblz) 
107                 - calx*calz*cblx*sblz);
108   rollRec = atan2(srzcrx / cos(-pitchRec), crzcrx / cos(-pitchRec));
109 
110   x1 = cos(rollRec) * txDiff - sin(rollRec) * tyDiff;
111   y1 = sin(rollRec) * txDiff + cos(rollRec) * tyDiff;
112   z1 = tzDiff;
113 
114   x2 = x1;
115   y2 = cos(pitchRec) * y1 + sin(pitchRec) * z1;
116   z2 = -sin(pitchRec) * y1 + cos(pitchRec) * z1;
117 
118   txDiff = cos(yawRec) * x2 - sin(yawRec) * z2;
119   tyDiff = y2;
120   tzDiff = sin(yawRec) * x2 + cos(yawRec) * z2;
121 
122   txRec = transformAftBA[3] + txDiff;
123   tyRec = transformAftBA[4] + tyDiff;
124   tzRec = transformAftBA[5] + tzDiff;
125 }
126 
127 void voDataHandler(const nav_msgs::Odometry::ConstPtr& voData)
128 {
129   if (fabs(timeOdomBefBA - timeOdomAftBA) < 0.005) {
130 
131     geometry_msgs::Quaternion geoQuat = voData->pose.pose.orientation;
132     tf::Matrix3x3(tf::Quaternion(geoQuat.z, -geoQuat.x, -geoQuat.y, geoQuat.w)).getRPY(rollRec, pitchRec, yawRec);
133 
134     txRec = voData->pose.pose.position.x;
135     tyRec = voData->pose.pose.position.y;
136     tzRec = voData->pose.pose.position.z;
137 
138     transformAssociateToBA();
139 
140     geoQuat = tf::createQuaternionMsgFromRollPitchYaw(rollRec, pitchRec, yawRec);
141 
142     voData2.header.stamp = voData->header.stamp;
143     //modified at 2018/01/18
144     //voData2.pose.pose.orientation.x = -geoQuat.y;
145     //voData2.pose.pose.orientation.y = -geoQuat.z;
146     //voData2.pose.pose.orientation.z = geoQuat.x;
147     //voData2.pose.pose.orientation.w = geoQuat.w;
148     //voData2.pose.pose.position.x = txRec;
149     //voData2.pose.pose.position.y = tyRec;
150     //voData2.pose.pose.position.z = tzRec;
151     //所有在tf中的应用,例如getRPY()和createQuaternionMsgFromRollPitchYaw()等都是在一般坐标系下进行的,所以想把voData转换到一般坐标系,只要按顺序将四元数按符号对应就可以了
152     //上面voData传进来的四元数geoQuat.x和geoQuat.y加了负号,所以pitchRec和yawRec是带负号的,因此将这些欧拉角重新转换为四元数后geoQuat.y和geoQuat.z是带负号的
153     //txRec~tzRec是指实际坐标系下的位移量(也就是z朝前,y向上,x向左),因此对应着转换到一般坐标系(x向前,y向左,z向上),tzRec对应着一般坐标系x轴的位移,txRec对应着一般坐标系y轴的位移
154     voData2.pose.pose.orientation.x = geoQuat.x;
155     voData2.pose.pose.orientation.y = -geoQuat.y;
156     voData2.pose.pose.orientation.z = -geoQuat.z;
157     voData2.pose.pose.orientation.w = geoQuat.w;
158     voData2.pose.pose.position.x = tzRec;
159     voData2.pose.pose.position.y = txRec;
160     voData2.pose.pose.position.z = tyRec;
161     //modigied at 2018/01/17
162     for(int conv_num=0;conv_num<36;conv_num++)
163       voData2.pose.covariance[conv_num]=conv[conv_num];
164     
165     voData2PubPointer->publish(voData2);
166 
167     voDataTrans2.stamp_ = voData->header.stamp;
168     voDataTrans2.setRotation(tf::Quaternion(-geoQuat.y, -geoQuat.z, geoQuat.x, geoQuat.w));
169     voDataTrans2.setOrigin(tf::Vector3(txRec, tyRec, tzRec));
170     tfBroadcaster2Pointer->sendTransform(voDataTrans2);
171   }
172 }
173 
174 void odomBefBAHandler(const nav_msgs::Odometry::ConstPtr& odomBefBA)
175 {
176   timeOdomBefBA = odomBefBA->header.stamp.toSec();
177 
178   double roll, pitch, yaw;
179   geometry_msgs::Quaternion geoQuat = odomBefBA->pose.pose.orientation;
180   tf::Matrix3x3(tf::Quaternion(geoQuat.z, -geoQuat.x, -geoQuat.y, geoQuat.w)).getRPY(roll, pitch, yaw);
181 
182   transformBefBA[0] = pitch;
183   transformBefBA[1] = yaw;
184   transformBefBA[2] = roll;
185 
186   transformBefBA[3] = odomBefBA->pose.pose.position.x;
187   transformBefBA[4] = odomBefBA->pose.pose.position.y;
188   transformBefBA[5] = odomBefBA->pose.pose.position.z;
189 }
190 
191 void odomAftBAHandler(const nav_msgs::Odometry::ConstPtr& odomAftBA)
192 {
193   timeOdomAftBA = odomAftBA->header.stamp.toSec();
194 
195   double roll, pitch, yaw;
196   geometry_msgs::Quaternion geoQuat = odomAftBA->pose.pose.orientation;
197   tf::Matrix3x3(tf::Quaternion(geoQuat.z, -geoQuat.x, -geoQuat.y, geoQuat.w)).getRPY(roll, pitch, yaw);
198 
199   transformAftBA[0] = pitch;
200   transformAftBA[1] = yaw;
201   transformAftBA[2] = roll;
202 
203   transformAftBA[3] = odomAftBA->pose.pose.position.x;
204   transformAftBA[4] = odomAftBA->pose.pose.position.y;
205   transformAftBA[5] = odomAftBA->pose.pose.position.z;
206 }
207 
208 int main(int argc, char** argv)
209 {
210   ros::init(argc, argv, "transformMaintenance");
211   ros::NodeHandle nh;
212 
213   ros::Subscriber voDataSub = nh.subscribe<nav_msgs::Odometry> 
214                               ("/cam_to_init", 1, voDataHandler);
215 
216   ros::Subscriber odomBefBASub = nh.subscribe<nav_msgs::Odometry> 
217                                  ("/bef_ba_to_init", 1, odomBefBAHandler);
218 
219   ros::Subscriber odomAftBASub = nh.subscribe<nav_msgs::Odometry> 
220                                  ("/aft_ba_to_init", 1, odomAftBAHandler);
221 
222   ros::Publisher voData2Pub = nh.advertise<nav_msgs::Odometry> ("/cam2_to_init", 1);
223   voData2PubPointer = &voData2Pub;
224   //modified at 2018/01/17
225   voData2.header.frame_id = "camera_init";//remove the leading slash
226   voData2.child_frame_id = "camera2";//remove the leading slash
227 
228   tf::TransformBroadcaster tfBroadcaster2;
229   tfBroadcaster2Pointer = &tfBroadcaster2;
230   voDataTrans2.frame_id_ = "camera_init";//remove the leading slash
231   voDataTrans2.child_frame_id_ = "camera2";//remove the leading slash
232 
233   ros::spin();
234 
235   return 0;
236 }

 

posted on 2018-04-07 22:31  zhch_pan  阅读(1546)  评论(0编辑  收藏  举报

导航