rplidar SDK 二次开发---之获取目标信息（0.1）

（x_{1},y_{1}）参考论文：

①https://blog.csdn.net/WisdomXLH/article/details/80548293

②https://blog.csdn.net/WisdomXLH/article/details/80556795

③https://blog.csdn.net/sunyoop/article/details/78302090

此程序在在SDK中的rplidar_sdk-mastersdkappultra_simple下进行修改，修改main.cpp文件。增加Target_orientation.cpp与Target_orientation.h文件。主要实现的功能是探测在固定的雷达扫面扇区内的目标物体的方位信息，距离信息，长度信息。

main.c文件：

//user header files
#include "Target_orientation.h"
#ifdef _WIN32
#include <Windows.h>
#define delay(x) ::Sleep(x)
#else
#include <unistd.h>
static inline void delay(_word_size_t ms){
while (ms>=1000){
usleep(1000*1000);
ms-=1000;
};
if (ms!=0)
usleep(ms*1000);
}
#endif

using namespace rp::standalone::rplidar;

bool checkRPLIDARHealth(RPlidarDriver * drv)
{
u_result op_result;
rplidar_response_device_health_t healthinfo;


op_result = drv->getHealth(healthinfo);
if (IS_OK(op_result)) { // the macro IS_OK is the preperred way to judge whether the operation is succeed.
printf("RPLidar health status : %d ", healthinfo.status);
if (healthinfo.status == RPLIDAR_STATUS_ERROR) {
fprintf(stderr, "Error, rplidar internal error detected. Please reboot the device to retry. ");
// enable the following code if you want rplidar to be reboot by software
// drv->reset();
return false;
} else {
return true;
}

} else {
fprintf(stderr, "Error, cannot retrieve the lidar health code: %x ", op_result);
return false;
}
}

#include <signal.h>
bool ctrl_c_pressed;
void ctrlc(int)
{
ctrl_c_pressed = true;
}

int main(int argc, const char * argv[]) {
const char * opt_com_path = NULL;
_u32 baudrateArray[2] = {115200, 256000};
_u32 opt_com_baudrate = 0;
u_result op_result;

double target_slope = 0;
double target_length = 0;
target_information_t target_information;

bool useArgcBaudrate = false;
/*
printf("Ultra simple LIDAR data grabber for RPLIDAR. "
"Version: "RPLIDAR_SDK_VERSION" ");
*/
printf("Ultra simple LIDAR data grabber for RPLIDAR. "
"Version: %s ", RPLIDAR_SDK_VERSION);
// read serial port from the command line...
if (argc>1) opt_com_path = argv[1]; // or set to a fixed value: e.g. "com3"

// read baud rate from the command line if specified...
if (argc>2)
{
opt_com_baudrate = strtoul(argv[2], NULL, 10);
useArgcBaudrate = true;
}

if (!opt_com_path) {
#ifdef _WIN32
// use default com port
opt_com_path = "\\.\com3";
#else
opt_com_path = "/dev/ttyUSB0";
#endif
}

// create the driver instance
RPlidarDriver * drv = RPlidarDriver::CreateDriver(DRIVER_TYPE_SERIALPORT);
if (!drv) {
fprintf(stderr, "insufficent memory, exit ");
exit(-2);
}

rplidar_response_device_info_t devinfo;
bool connectSuccess = false;
// make connection...
if(useArgcBaudrate)
{
if(!drv)
drv = RPlidarDriver::CreateDriver(DRIVER_TYPE_SERIALPORT);
if (IS_OK(drv->connect(opt_com_path, opt_com_baudrate)))
{
op_result = drv->getDeviceInfo(devinfo);

if (IS_OK(op_result))
{
connectSuccess = true;
}
else
{
delete drv;
drv = NULL;
}
}
}
else
{
size_t baudRateArraySize = (sizeof(baudrateArray))/ (sizeof(baudrateArray[0]));
for(size_t i = 0; i < baudRateArraySize; ++i)
{
if(!drv)
drv = RPlidarDriver::CreateDriver(DRIVER_TYPE_SERIALPORT);
if(IS_OK(drv->connect(opt_com_path, baudrateArray[i])))
{
op_result = drv->getDeviceInfo(devinfo);

if (IS_OK(op_result))
{
connectSuccess = true;
break;
}
else
{
delete drv;
drv = NULL;
}
}
}
}
if (!connectSuccess) {

fprintf(stderr, "Error, cannot bind to the specified serial port %s. "
, opt_com_path);
goto on_finished;
}

// print out the device serial number, firmware and hardware version number..
printf("RPLIDAR S/N: ");
for (int pos = 0; pos < 16 ;++pos) {
printf("%02X", devinfo.serialnum[pos]);
}

printf(" "
"Firmware Ver: %d.%02d "
"Hardware Rev: %d "
, devinfo.firmware_version>>8
, devinfo.firmware_version & 0xFF
, (int)devinfo.hardware_version);



// check health...
if (!checkRPLIDARHealth(drv)) {
goto on_finished;
}

signal(SIGINT, ctrlc);

drv->startMotor();
// start scan...
drv->startScan(0,1);
//delay(10);
// fetech result and print it out...
while (1) {
rplidar_response_measurement_node_t nodes[8192];
modify_rplidar_response_measurement_node_t modify_nodes[500];
size_t count = _count_of(nodes);
//printf("count is :%d ", count);
op_result = drv->grabScanData(nodes, count);
//printf("count is :%d ",count);

if (IS_OK(op_result)) {
drv->ascendScanData(nodes, count);
//printf("count is %d ", count);
Obtaining_Index_Boundary_SectorArea(nodes, count);
//printf("Sector_One_Upper_Limit_Idex is %d ", Sector_One_Upper_Limit_Idex);
//printf("Sector_Two_Lower_Limit_Idex is %d ", Sector_Two_Lower_Limit_Idex);
//modify_rplidar_response_measurement_node_t modify_nodes[Sector_Total_Num];
Crop_ScanData(nodes, count, modify_nodes);


//打印剪切后的数据
/*
printf("start ");
for (int pos = 0; pos < Crop_Sector_Total_Num; ++pos) {
printf("theta: %03.2f Dist: %04.2f ",
(modify_nodes[pos].angle_q6_checkbit >> RPLIDAR_RESP_MEASUREMENT_ANGLE_SHIFT) / 64.0f,
modify_nodes[pos].distance_q2 / 4.0f);
}
*/
/*
for (int pos = 0; pos < (int)count ; ++pos) {
printf("%s theta: %03.2f Dist: %08.2f Q: %d ",
(nodes[pos].sync_quality & RPLIDAR_RESP_MEASUREMENT_SYNCBIT) ?"S ":" ",
(nodes[pos].angle_q6_checkbit >> RPLIDAR_RESP_MEASUREMENT_ANGLE_SHIFT)/64.0f,
nodes[pos].distance_q2/4.0f,
nodes[pos].sync_quality >> RPLIDAR_RESP_MEASUREMENT_QUALITY_SHIFT);

}
*/

//获取扇形区目标边沿的点在雷达坐标系中的坐标（x,y）
vector <point_t> target_points(Crop_Sector_Total_Num);
//printf("new data begin ");
for (int i = 0; i < Crop_Sector_Total_Num; i++)
{
target_points[i].x = (modify_nodes[i].distance_q2 / 4.0f) * sin((modify_nodes[i].angle_q6_checkbit >> RPLIDAR_RESP_MEASUREMENT_ANGLE_SHIFT) / 64.0f / 180 * M_PI);
target_points[i].y = (modify_nodes[i].distance_q2 / 4.0f) * cos((modify_nodes[i].angle_q6_checkbit >> RPLIDAR_RESP_MEASUREMENT_ANGLE_SHIFT) / 64.0f / 180 * M_PI);
//printf("x: %05.2f y:%05.2f ", target_points[i].x, target_points[i].y);
}
point_t *buffer = new point_t[target_points.size()];
if (!target_points.empty())
{
memcpy(buffer, &target_points[0], target_points.size() * sizeof(point_t));
/*
printf("new data begin ");
for (int i = 0; i < Crop_Sector_Total_Num; i++)
{
printf("x: %08.2f y:%08.2f ", buffer[i].x, buffer[i].y);
}
*/

}
int target_center_index = (floor(Crop_Sector_Total_Num / 2));
target_information.distance = modify_nodes[target_center_index].distance_q2 / 4.0f /100;

target_information.target_slope = LeastSquaresFitting(buffer, Crop_Sector_Total_Num);
//printf("target_slope is %f ", target_information.target_slope);

target_information.target_length = distance_sqr(buffer, &buffer[Crop_Sector_Total_Num-1]);
//printf("target length is %f ", target_information.target_length);

printf("distace: %f slope: %f length: %f ", target_information.distance, target_information.target_slope, target_information.target_length);
delete[]buffer;
}

if (ctrl_c_pressed){
break;
}
}

drv->stop();
drv->stopMotor();
// done!
on_finished:
RPlidarDriver::DisposeDriver(drv);
drv = NULL;
return 0;
}

Target_orientation.h文件：

#pragma once
/*
**********************************************************************************************************
INCLUDES
**********************************************************************************************************
*/
#include<iostream>
#include<vector>
#include <algorithm>

#include <stdio.h>
#include <stdlib.h>
#include <float.h>

#define _USE_MATH_DEFINES
#include <math.h>

#include "rplidar.h" //RPLIDAR standard sdk, all-in-one header
#include "rptypes.h"

using namespace std;
/*
**********************************************************************************************************
GLOBAL VARIABLES
**********************************************************************************************************
*/
extern int Sector_One_Upper_Limit_Idex;
extern int Sector_Two_Lower_Limit_Idex;
extern int Sector_Total_Num;
extern int Crop_Sector_Total_Num;
/*
**********************************************************************************************************
MACROS
**********************************************************************************************************
*/
/*
选取是laser 正前方的扇型数据
扇区1：0° ~ 60°
扇区2：300°~ 360°
*/


#define Sector_One_Upper_Limit 30
#define Sector_Two_Lower_Limit 330

#ifndef _count_of
#define _count_of(_Array) (int)(sizeof(_Array) / sizeof(_Array[0]))
#endif
/*
**********************************************************************************************************
TYPEDEFS
**********************************************************************************************************
*/

struct point_t {
double x, y;
};

typedef struct _target_information_t {
double target_slope; // 相对于雷达坐标的倾角
double target_length; //目标物长度
double distance; // 目标中心相到雷达坐标系距离雷达中心点的距离
} __attribute__((packed)) target_information_t;

typedef struct _modify_rplidar_response_measurement_node_t {
_u16 angle_q6_checkbit; // check_bit:1;angle_q6:15;
_u16 distance_q2;
} __attribute__((packed)) modify_rplidar_response_measurement_node_t;

/*
**********************************************************************************************************
FUNCTIONS
**********************************************************************************************************
*/
extern double distance_sqr(struct point_t const*a, struct point_t const*b);
extern double LeastSquaresFitting(point_t Laser_Data[], int Data_Len);
extern void Obtaining_Index_Boundary_SectorArea(rplidar_response_measurement_node_t Nodes[], int Nodes_Len);
extern void Crop_ScanData(rplidar_response_measurement_node_t Nodes[], int Nodes_Len, modify_rplidar_response_measurement_node_t Modify_Nodes[]);

Target_orientation.cpp文件：

/*
**********************************************************************************************************
INCLUDES
**********************************************************************************************************
*/
#include "Target_orientation.h"
/*
**********************************************************************************************************
CONSTANTS
**********************************************************************************************************
* /
/*
**********************************************************************************************************
GLOBAL VARIABLES
**********************************************************************************************************
*/
int Sector_One_Upper_Limit_Idex = 0;
int Sector_Two_Lower_Limit_Idex = 0;
int Sector_Total_Num = 0;
int Crop_Sector_Total_Num = 0;
/*
**********************************************************************************************************
FUNCTIONS
**********************************************************************************************************
*/

double distance_sqr(struct point_t const*a, struct point_t const*b)
{
double ax, ay, bx, by;
ax = a->x /1000.0;
bx = b->x/ 1000.0;

ay = a->y /1000.0;
by = b->y / 1000.0;


return sqrtf((ax - bx) * (ax - bx) + (ay - by) * (ay - by));
}

/*
函 数 名：float LeastSquaresFitting
函数功能：最小二乘法拟合直线
输入参数：砖块边缘点位于雷达坐标中的坐标数组:point_t Laser_Data[]
输出参数：砖块边缘的斜率:slope
*/
double LeastSquaresFitting(point_t Laser_Data[],int Data_Len)
//float LeastSquaresFitting(vector <point_t> Laser_Data(), int Data_Len)
{
double slope; //拟合直线的斜率
double av_x, av_y;
double L_xx, L_xy;
//int Data_Len = _count_of(Laser_Data);
av_x = 0; //X的平均值
av_y = 0; //Y的平均值
L_xx = 0;
L_xy = 0;
int i = 0;
for (i = 0; i < Data_Len; i++) //计算X、Y的平均值
{
av_x += Laser_Data[i].x;
av_y += Laser_Data[i].y;
}
av_x = av_x / Data_Len;
av_y = av_y / Data_Len;

for (i = 0; i < Data_Len; i++) //计算Lxx、Lyy和Lxy
{
L_xx += (Laser_Data[i].x - av_x)*(Laser_Data[i].x - av_x);
L_xy += (Laser_Data[i].x - av_x)*(Laser_Data[i].y - av_y);
}

slope = L_xy / L_xx; //斜率
return slope;
}

/*
函 数 名：Obtaining_Index_Boundary_SectorArea
函数功能：获得感兴趣扫描区域边界数据索引
输入参数：按角度递增排序的扫描数据:Nodes[];数据长度：Nodes_Len
输出参数：void
*/
void Obtaining_Index_Boundary_SectorArea(rplidar_response_measurement_node_t Nodes[], int Nodes_Len)
{
double previous_angle,next_angle;
previous_angle = (Nodes[0].angle_q6_checkbit >> RPLIDAR_RESP_MEASUREMENT_ANGLE_SHIFT) / 64.0f;
for (int pos = 1; pos < (int)Nodes_Len; ++pos)
{
next_angle = (Nodes[pos].angle_q6_checkbit >> RPLIDAR_RESP_MEASUREMENT_ANGLE_SHIFT) / 64.0f;
if (previous_angle <= Sector_One_Upper_Limit && Sector_One_Upper_Limit <= next_angle)
{
Sector_One_Upper_Limit_Idex = pos - 1;
}
if (previous_angle <= Sector_Two_Lower_Limit && Sector_Two_Lower_Limit <= next_angle)
{
Sector_Two_Lower_Limit_Idex = pos;
Sector_Total_Num = Nodes_Len - Sector_Two_Lower_Limit_Idex + Sector_One_Upper_Limit_Idex;
break;
}
previous_angle = next_angle;
}

}

/*
函 数 名：Crop_ScanData
函数功能：裁剪rplidar的扫描数据
输入参数：按角度递增排序的扫描数据:Nodes[];数据长度：Nodes_Len；剪裁后的扫描数据指针
输出参数：void
*/
void Crop_ScanData(rplidar_response_measurement_node_t Nodes[], int Nodes_Len, modify_rplidar_response_measurement_node_t Modify_Nodes[])
{
int i = 0, j = 0;
for (i = Sector_Two_Lower_Limit_Idex; i < Nodes_Len; i++)
{
if (Nodes[i].distance_q2 != 0)
{
Modify_Nodes[j].angle_q6_checkbit = Nodes[i].angle_q6_checkbit;
Modify_Nodes[j].distance_q2 = Nodes[i].distance_q2;
j += 1;
}
}

for (i = 0; i < Sector_One_Upper_Limit_Idex; i++)
{
if (Nodes[i].distance_q2 != 0)
{
Modify_Nodes[j].angle_q6_checkbit = Nodes[i].angle_q6_checkbit;
Modify_Nodes[j].distance_q2 = Nodes[i].distance_q2;
j += 1;
}
}

Crop_Sector_Total_Num = j;
}

其中，对目标边缘线的拟合并求取边缘在在雷达坐标系中的斜率用到了最小二乘法。最小二乘法（又称最小平方法）是一种数学优化技术。它通过最小化误差的平方和寻找数据的最佳函数匹配。利用最小二乘法可以简便地求得未知的数据，并使得这些求得的数据与实际数据之间误差的平方和为最 小，简单来说，就是通过最小化误差的平方和，使得拟合对象无限接近目标对象，这就是最小二乘的核心思想。最小二乘法还可用于曲线拟合。

公式推导：

假设存在一组二维数据，(x_{1},y_{1})(x_{2},y_{2})...(x_{n},y_{n}),设拟合直线为y=ax+b,则误差为d_{i}=y_{i}-(ax_{i}+b)。

最小二乘法：

D_i=sum_{ i=1}^{n}d_i^2 =sum_{ i=1}^{n}(y_{i}-(ax_{i}+b))^2,令该等式对a和b求偏导得到如下结果：

formula，formula。实现代码见Target_orientation.cpp文件中的函数LeastSquaresFittin()。

原文链接：https://blog.csdn.net/u011591807/article/details/94406609