loam source address: https://github.com/cuitaixiang/LOAM_NOTED.
Thesis study: Loam: Lidar odometry and mapping in real time.
Analysis and summary of loam source code:
loam source code analysis 1: scanRegistration (I).
loam source code analysis 2: scanRegistration (II).
loam source code analysis 3: laserOdometry (I).
loam source code analysis 4: lasero dometry (II).
loam source code analysis 5: lasero dometry (III).
loam source code analysis 6: laserMapping (I).
loam source code analysis 7: laserMapping (II).
loam source code analysis 8: transformMaintenance.
1, Overview
So far, we have completed the code analysis of the first three parts and entered the last part, transformation maintenance. This part is also very simple, without any algorithm and detail difficulties. It is mainly to receive and release information, less nonsense and look at the code directly.
2, Variable description
Odometer information from laser odometry (high frequency):
//Transfer matrix calculated by odometry (real-time high-frequency quantity)
float transformSum[6] = {0};
//Translation increment
float transformIncre[6] = {0};
Odometer information (low frequency) from laserMapping, optimized odometer information, map:
//The final pose in the world coordinate system after mapping correction
float transformMapped[6] = {0};
//The pose before optimization passed from mapping
float transformBefMapped[6] = {0};
//The optimized pose passed from mapping
float transformAftMapped[6] = {0};
Define the release of ROS and TF:
ros::Publisher *pubLaserOdometry2Pointer = NULL; tf::TransformBroadcaster *tfBroadcaster2Pointer = NULL; nav_msgs::Odometry laserOdometry2; tf::StampedTransform laserOdometryTrans2;
3, Information receiving and processing
int main(int argc, char** argv)
{
ros::init(argc, argv, "transformMaintenance");
ros::NodeHandle nh;
ros::Subscriber subLaserOdometry = nh.subscribe<nav_msgs::Odometry>
("/laser_odom_to_init", 5, laserOdometryHandler);
ros::Subscriber subOdomAftMapped = nh.subscribe<nav_msgs::Odometry>
("/aft_mapped_to_init", 5, odomAftMappedHandler);
1. transformAssociateToMap pose fusion
This part is related to loam source code analysis 6: laserMapping (I) The transformAssociateToMap () as like as two peas in the same way, the function is the final pose transformMapped obtained from the motion estimation of odometry and the integration of mapping correction. The only difference is that the initial estimate is obtained in laserMapping. T ˉ k W ( t k + 1 ) \bar T^W_{k}(t_{k+1}) T ˉ kW (tk+1), and here is the result of laserMapping iterative optimization T k W ( t k + 1 ) T^W_{k}(t_{k+1}) TkW (tk+1), so I won't repeat it.
//The final pose transformMapped is obtained after the fusion of motion estimation of odometry and mapping correction
void transformAssociateToMap()
{
//Rotate around y axis after translation (- transformSum[1])
float x1 = cos(transformSum[1]) * (transformBefMapped[3] - transformSum[3])
- sin(transformSum[1]) * (transformBefMapped[5] - transformSum[5]);
float y1 = transformBefMapped[4] - transformSum[4];
float z1 = sin(transformSum[1]) * (transformBefMapped[3] - transformSum[3])
+ cos(transformSum[1]) * (transformBefMapped[5] - transformSum[5]);
//Rotate around the x axis (- transformSum[0])
float x2 = x1;
float y2 = cos(transformSum[0]) * y1 + sin(transformSum[0]) * z1;
float z2 = -sin(transformSum[0]) * y1 + cos(transformSum[0]) * z1;
//Rotate around the z axis (- transformSum[2])
transformIncre[3] = cos(transformSum[2]) * x2 + sin(transformSum[2]) * y2;
transformIncre[4] = -sin(transformSum[2]) * x2 + cos(transformSum[2]) * y2;
transformIncre[5] = z2;
float sbcx = sin(transformSum[0]);
float cbcx = cos(transformSum[0]);
float sbcy = sin(transformSum[1]);
float cbcy = cos(transformSum[1]);
float sbcz = sin(transformSum[2]);
float cbcz = cos(transformSum[2]);
float sblx = sin(transformBefMapped[0]);
float cblx = cos(transformBefMapped[0]);
float sbly = sin(transformBefMapped[1]);
float cbly = cos(transformBefMapped[1]);
float sblz = sin(transformBefMapped[2]);
float cblz = cos(transformBefMapped[2]);
float salx = sin(transformAftMapped[0]);
float calx = cos(transformAftMapped[0]);
float saly = sin(transformAftMapped[1]);
float caly = cos(transformAftMapped[1]);
float salz = sin(transformAftMapped[2]);
float calz = cos(transformAftMapped[2]);
float srx = -sbcx*(salx*sblx + calx*cblx*salz*sblz + calx*calz*cblx*cblz)
- cbcx*sbcy*(calx*calz*(cbly*sblz - cblz*sblx*sbly)
- calx*salz*(cbly*cblz + sblx*sbly*sblz) + cblx*salx*sbly)
- cbcx*cbcy*(calx*salz*(cblz*sbly - cbly*sblx*sblz)
- calx*calz*(sbly*sblz + cbly*cblz*sblx) + cblx*cbly*salx);
transformMapped[0] = -asin(srx);
float srycrx = sbcx*(cblx*cblz*(caly*salz - calz*salx*saly)
- cblx*sblz*(caly*calz + salx*saly*salz) + calx*saly*sblx)
- cbcx*cbcy*((caly*calz + salx*saly*salz)*(cblz*sbly - cbly*sblx*sblz)
+ (caly*salz - calz*salx*saly)*(sbly*sblz + cbly*cblz*sblx) - calx*cblx*cbly*saly)
+ cbcx*sbcy*((caly*calz + salx*saly*salz)*(cbly*cblz + sblx*sbly*sblz)
+ (caly*salz - calz*salx*saly)*(cbly*sblz - cblz*sblx*sbly) + calx*cblx*saly*sbly);
float crycrx = sbcx*(cblx*sblz*(calz*saly - caly*salx*salz)
- cblx*cblz*(saly*salz + caly*calz*salx) + calx*caly*sblx)
+ cbcx*cbcy*((saly*salz + caly*calz*salx)*(sbly*sblz + cbly*cblz*sblx)
+ (calz*saly - caly*salx*salz)*(cblz*sbly - cbly*sblx*sblz) + calx*caly*cblx*cbly)
- cbcx*sbcy*((saly*salz + caly*calz*salx)*(cbly*sblz - cblz*sblx*sbly)
+ (calz*saly - caly*salx*salz)*(cbly*cblz + sblx*sbly*sblz) - calx*caly*cblx*sbly);
transformMapped[1] = atan2(srycrx / cos(transformMapped[0]),
crycrx / cos(transformMapped[0]));
float srzcrx = (cbcz*sbcy - cbcy*sbcx*sbcz)*(calx*salz*(cblz*sbly - cbly*sblx*sblz)
- calx*calz*(sbly*sblz + cbly*cblz*sblx) + cblx*cbly*salx)
- (cbcy*cbcz + sbcx*sbcy*sbcz)*(calx*calz*(cbly*sblz - cblz*sblx*sbly)
- calx*salz*(cbly*cblz + sblx*sbly*sblz) + cblx*salx*sbly)
+ cbcx*sbcz*(salx*sblx + calx*cblx*salz*sblz + calx*calz*cblx*cblz);
float crzcrx = (cbcy*sbcz - cbcz*sbcx*sbcy)*(calx*calz*(cbly*sblz - cblz*sblx*sbly)
- calx*salz*(cbly*cblz + sblx*sbly*sblz) + cblx*salx*sbly)
- (sbcy*sbcz + cbcy*cbcz*sbcx)*(calx*salz*(cblz*sbly - cbly*sblx*sblz)
- calx*calz*(sbly*sblz + cbly*cblz*sblx) + cblx*cbly*salx)
+ cbcx*cbcz*(salx*sblx + calx*cblx*salz*sblz + calx*calz*cblx*cblz);
transformMapped[2] = atan2(srzcrx / cos(transformMapped[0]),
crzcrx / cos(transformMapped[0]));
x1 = cos(transformMapped[2]) * transformIncre[3] - sin(transformMapped[2]) * transformIncre[4];
y1 = sin(transformMapped[2]) * transformIncre[3] + cos(transformMapped[2]) * transformIncre[4];
z1 = transformIncre[5];
x2 = x1;
y2 = cos(transformMapped[0]) * y1 - sin(transformMapped[0]) * z1;
z2 = sin(transformMapped[0]) * y1 + cos(transformMapped[0]) * z1;
transformMapped[3] = transformAftMapped[3]
- (cos(transformMapped[1]) * x2 + sin(transformMapped[1]) * z2);
transformMapped[4] = transformAftMapped[4] - y2;
transformMapped[5] = transformAftMapped[5]
- (-sin(transformMapped[1]) * x2 + cos(transformMapped[1]) * z2);
}
2. laserOdometryHandler odometer information processing
Decompose the received odometer information attitude into rotation and Translation:
//Receive information from laserOdometry
void laserOdometryHandler(const nav_msgs::Odometry::ConstPtr& laserOdometry)
{
double roll, pitch, yaw;
geometry_msgs::Quaternion geoQuat = laserOdometry->pose.pose.orientation;
tf::Matrix3x3(tf::Quaternion(geoQuat.z, -geoQuat.x, -geoQuat.y, geoQuat.w)).getRPY(roll, pitch, yaw);
//The rotation translation matrix is obtained
transformSum[0] = -pitch;
transformSum[1] = -yaw;
transformSum[2] = roll;
transformSum[3] = laserOdometry->pose.pose.position.x;
transformSum[4] = laserOdometry->pose.pose.position.y;
transformSum[5] = laserOdometry->pose.pose.position.z;
Convert the attitude from the current frame coordinate system to the map coordinate system:
transformAssociateToMap();
Publish the current pose ROS after conversion to the map coordinate system:
geoQuat = tf::createQuaternionMsgFromRollPitchYaw
(transformMapped[2], -transformMapped[0], -transformMapped[1]);
laserOdometry2.header.stamp = laserOdometry->header.stamp;
laserOdometry2.pose.pose.orientation.x = -geoQuat.y;
laserOdometry2.pose.pose.orientation.y = -geoQuat.z;
laserOdometry2.pose.pose.orientation.z = geoQuat.x;
laserOdometry2.pose.pose.orientation.w = geoQuat.w;
laserOdometry2.pose.pose.position.x = transformMapped[3];
laserOdometry2.pose.pose.position.y = transformMapped[4];
laserOdometry2.pose.pose.position.z = transformMapped[5];
pubLaserOdometry2Pointer->publish(laserOdometry2);
Publish the current pose TF after conversion to the map coordinate system:
//Send rotation translation amount laserOdometryTrans2.stamp_ = laserOdometry->header.stamp; laserOdometryTrans2.setRotation(tf::Quaternion(-geoQuat.y, -geoQuat.z, geoQuat.x, geoQuat.w)); laserOdometryTrans2.setOrigin(tf::Vector3(transformMapped[3], transformMapped[4], transformMapped[5])); tfBroadcaster2Pointer->sendTransform(laserOdometryTrans2); }
3. odomAftMappedHandler map information processing
Accept the optimized pose from laserMapping T ˉ k W ( t k + 1 ) \bar T^W_{k}(t_{k+1}) TˉkW(tk+1):
//Receive conversion information of laserMapping
void odomAftMappedHandler(const nav_msgs::Odometry::ConstPtr& odomAftMapped)
{
double roll, pitch, yaw;
geometry_msgs::Quaternion geoQuat = odomAftMapped->pose.pose.orientation;
tf::Matrix3x3(tf::Quaternion(geoQuat.z, -geoQuat.x, -geoQuat.y, geoQuat.w)).getRPY(roll, pitch, yaw);
transformAftMapped[0] = -pitch;
transformAftMapped[1] = -yaw;
transformAftMapped[2] = roll;
transformAftMapped[3] = odomAftMapped->pose.pose.position.x;
transformAftMapped[4] = odomAftMapped->pose.pose.position.y;
transformAftMapped[5] = odomAftMapped->pose.pose.position.z;
transformBefMapped[0] = odomAftMapped->twist.twist.angular.x;
transformBefMapped[1] = odomAftMapped->twist.twist.angular.y;
transformBefMapped[2] = odomAftMapped->twist.twist.angular.z;
transformBefMapped[3] = odomAftMapped->twist.twist.linear.x;
transformBefMapped[4] = odomAftMapped->twist.twist.linear.y;
transformBefMapped[5] = odomAftMapped->twist.twist.linear.z;
}
3, Information sending and processing
Two publishing instances are defined:
ros::Publisher pubLaserOdometry2 = nh.advertise<nav_msgs::Odometry> ("/integrated_to_init", 5);
pubLaserOdometry2Pointer = &pubLaserOdometry2;
laserOdometry2.header.frame_id = "/camera_init";
laserOdometry2.child_frame_id = "/camera";
tf::TransformBroadcaster tfBroadcaster2;
tfBroadcaster2Pointer = &tfBroadcaster2;
laserOdometryTrans2.frame_id_ = "/camera_init";
laserOdometryTrans2.child_frame_id_ = "/camera";
ros::spin();
return 0;
}
4, Conclusion
Finally, I wrote here. If you also see here, congratulations to you and me. The study of loam has come to an end. The reason why I record this learning process on my blog is that it is convenient to check it. I hope I can turn it often, but I also hope I can stop turning it. I'm afraid of endless truth and have an inch of joy every inch. Logo-beam, go!