(Ros) LSM을 이용한 Lane Fitting

on Projects

LSM,최소자승법

1. LSM

어떤 점들의 분포를 직선 또는 곡선으로 근사하는 방법.

1.1. 직선모델

(x,y)데이터에 대해 임의의 직선 f(x) = ax + b를 모델로 설정하면 a,b가 파라미터가 된다.
실제 y값과 f(x)값의 residual^2의 차이가 최소가 되도록 a,b를 결정한다.
식으로 나타내면 다음과 같다.
image image 위의 식이 최소가 되도록 하는 a,b를 결정해야한다.

위 식을 행렬로 나타내면 표현하면 다음과 같다. image 왼쪽이 오른쪽이랑 같아지는 (a b)^T를 구하면 된다.

image

1.1.1. Code

#include <ros/ros.h>
#include <pcl/point_types.h>
#include <pcl_conversions/pcl_conversions.h>
#include <visualization_msgs/Marker.h>
#include <cmath>

ros::Publisher pub;

void CallbackFunction(const sensor_msgs::PointCloud2::ConstPtr & msg )
{
pcl::PointCloud<pcl::PointXYZ> cloud;
pcl::fromROSMsg(*msg,cloud);

double x_mean, y_mean, num, den, a, b;
int len = cloud.size();

for(int i=0; i<len; i++)
{
x_mean += cloud.points[i].x;
y_mean += cloud.points[i].y;
}
x_mean = x_mean/len;
y_mean = y_mean/len;

for(int i=0; i<len; i++)
{
num += (cloud.points[i].y - y_mean)*(cloud.points[i].x - x_mean);
den += (cloud.points[i].x - x_mean)*(cloud.points[i].x - x_mean);
}
a = num/den;
b = y_mean - a * x_mean;




visualization_msgs::Marker line_strip;
line_strip.header.frame_id = "/map";
line_strip.header.stamp = ros::Time::now();
line_strip.ns = "points_and_lines";
line_strip.action = visualization_msgs::Marker::ADD;
line_strip.pose.orientation.w = 1.0;
line_strip.type = visualization_msgs::Marker::LINE_STRIP;
line_strip.scale.x = 0.1;
line_strip.color.b = 1.0;
line_strip.color.a = 1.0;

geometry_msgs::Point p;
for(int x=-5; x<6; x++)
{
p.x = x;
p.y = a*x + b;
p.z = 0;

line_strip.points.push_back(p);
}




pub.publish(line_strip);

}





int main(int argc, char** argv)
{
ros::init(argc, argv, "laneFitting");
ros::NodeHandle nh;
ros::Subscriber sub = nh.subscribe<sensor_msgs::PointCloud2>("/input",10,CallbackFunction);
pub = nh.advertise<visualization_msgs::Marker>("/lane",10);
ros::spin();
return 0;
}

1.1.2. tutorial

다음과 같은 raw 데이터에 LSM으로 Lane Fitting 하기 image

image

1.2. 곡선모델

image 마찬가지로 툴을 이용해 pseudo inverse(유사역행렬)해주면 된다.