转载请注明出处：本文转自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 }