void LinearRegression::Learn(const std::vector<double>& input, const std::vector<double>& output, int dataSize)
{
Reset();
mInputDim = input.size() / dataSize;
mOutputDim = output.size() / dataSize;
Eigen::MatrixXd matA(dataSize, mInputDim + 1);
Eigen::MatrixXd matB(dataSize, mOutputDim);
for (int dataId = 0; dataId < dataSize; dataId++)
{
int inputBase = dataId * mInputDim;
for (int dimId = 0; dimId < mInputDim; dimId++)
{
matA(dataId, dimId) = input.at(inputBase + dimId);
}
matA(dataId, mInputDim) = 1.0;
int outputBase = dataId * mOutputDim;
for (int dimId = 0; dimId < mOutputDim; dimId++)
{
matB(dataId, dimId) = output.at(outputBase + dimId);
}
}
Eigen::MatrixXd matAT = matA.transpose();
Eigen::MatrixXd matCoefA = matAT * matA;
Eigen::MatrixXd matCoefB = matAT * matB;
Eigen::LDLT<Eigen::MatrixXd> solver;
solver.compute(matCoefA);
Eigen::MatrixXd matRes = solver.solve(matCoefB);
//copy result
mRegMat.resize((mInputDim + 1) * mOutputDim);
for (int colId = 0; colId < mOutputDim; colId++)
{
int baseIndex = colId * (mInputDim + 1);
for (int rowId = 0; rowId <= mInputDim; rowId++)
{
mRegMat.at(baseIndex + rowId) = matRes(rowId, colId);
}
}
}
void popblas::testBlas::test_dot() {
pop::F32 dataA[] = {1, 2, 1,
2, 4, 5,
2, 2, 1};
pop::F32 dataB[] = {2, 0, 0,
1, 2, 2,
3, 1, 2};
pop::MatN<2, pop::F32> matA(pop::VecN<2, int>(3, 3), dataA);
pop::MatN<2, pop::F32> matB(pop::VecN<2, int>(3, 3), dataB);
std::cout << blas::dot(matA, matB) << std::endl;
// pop::MatN<2, pop::F32> vecA = matA.selectRow(0);
// pop::MatN<2, pop::F32> vecB = matB.selectColumn(0);
// std::cout << blas::dot(vecA, vecB) << std::endl;
}
int main()
{
int ii = 4;
int jj = 3;
int kk = 2;
int ll = 5;
int mm = 6;
int nn = 7;
quad_type matA(ll,kk,jj,ii);
quad_type matB(nn,mm,jj,ii);
quad_type matC(ll,kk,nn,mm);
fill_random(matB);
fill_random(matC);
//fill_from_file(matB, "B.mat");
//fill_from_file(matC, "C.mat");
libmda::cindex<'i'> i;
libmda::cindex<'j'> j;
libmda::cindex<'k'> k;
libmda::cindex<'l'> l;
libmda::cindex<'m'> m;
libmda::cindex<'n'> n;
auto start = std::chrono::steady_clock::now();
matA(l,k,j,i) = matB(n,m,j,i) * matC(l,k,n,m);
auto stop = std::chrono::steady_clock::now();
auto diff = stop - start;
std::cout << std::chrono::duration<double, std::milli>(diff).count() << "ms" << std::endl;
print(matA, "A.data");
print(matB, "B.data");
print(matC, "C.data");
return 0;
}
void popblas::testBlas::test_ger() {
pop::F32 dataX[] = {2,3,4};
pop::F32 datamatX[] = {2, 3, 4,
1, 5, 3,
2, 1, 0};
pop::F32 dataY[] = {1,3,5,7,2};
pop::F32 alpha = 2;
pop::MatN<2, pop::F32> vecX(pop::VecN<2, int>(1, 3), dataX);
pop::MatN<2, pop::F32> matX(pop::VecN<2, int>(3, 3), datamatX);
pop::MatN<2, pop::F32> vecY(pop::VecN<2, int>(1, 5), dataY);
pop::MatN<2, pop::F32> matA(3, 5);
pop::MatN<2, pop::F32> matB(3, 5);
matA = 0; matB = 0;
blas::ger(alpha, vecX, vecY, matA);
pop::MatN<2, pop::F32> vecmatX = matX.selectRow(0);
blas::ger(alpha, vecmatX, vecY, matB);
std::cout << matA << std::endl;
std::cout << matB << std::endl;
}
void popblas::testBlas::test_gemm() {
pop::F32 alpha = 1;
pop::F32 beta = 1;
pop::F32 dataA[] = {0, 3, 2,
1, -2, 3,
5, 6, 2,
0, 1, 2,
3, 6, 1};
pop::F32 dataB[] = {1, 3, 2, 4,
2, 2, 0, 5,
0, 1, 7, 2};
pop::MatN<2, pop::F32> matA(pop::VecN<2, int>(5, 3), dataA);
pop::MatN<2, pop::F32> opMatA = matA.transpose();
pop::MatN<2, pop::F32> matB(pop::VecN<2, int>(3, 4), dataB);
pop::MatN<2, pop::F32> opMatB = matB.transpose();
pop::MatN<2, pop::F32> matC(5, 4);
matC = 0;
blas::gemm(alpha, opMatA, 'T', opMatB, 'T', beta, matC);
std::cout << matC << std::endl;
}
ViennaCLStatus ViennaCLHostgemm_impl(ViennaCLBackend /*backend*/,
ViennaCLOrder orderA, ViennaCLTranspose transA,
ViennaCLOrder orderB, ViennaCLTranspose transB,
ViennaCLOrder orderC,
ViennaCLInt m, ViennaCLInt n, ViennaCLInt k,
NumericT alpha,
NumericT *A, ViennaCLInt offA_row, ViennaCLInt offA_col, ViennaCLInt incA_row, ViennaCLInt incA_col, ViennaCLInt lda,
NumericT *B, ViennaCLInt offB_row, ViennaCLInt offB_col, ViennaCLInt incB_row, ViennaCLInt incB_col, ViennaCLInt ldb,
NumericT beta,
NumericT *C, ViennaCLInt offC_row, ViennaCLInt offC_col, ViennaCLInt incC_row, ViennaCLInt incC_col, ViennaCLInt ldc)
{
typedef typename viennacl::matrix_base<NumericT>::size_type size_type;
typedef typename viennacl::matrix_base<NumericT>::difference_type difference_type;
size_type A_size1 = static_cast<size_type>((transA == ViennaCLTrans) ? k : m);
size_type A_size2 = static_cast<size_type>((transA == ViennaCLTrans) ? m : k);
size_type B_size1 = static_cast<size_type>((transB == ViennaCLTrans) ? n : k);
size_type B_size2 = static_cast<size_type>((transB == ViennaCLTrans) ? k : n);
bool A_row_major = (orderA == ViennaCLRowMajor);
bool B_row_major = (orderB == ViennaCLRowMajor);
bool C_row_major = (orderC == ViennaCLRowMajor);
viennacl::matrix_base<NumericT> matA(A, viennacl::MAIN_MEMORY,
A_size1, size_type(offA_row), difference_type(incA_row), size_type(A_row_major ? m : lda),
A_size2, size_type(offA_col), difference_type(incA_col), size_type(A_row_major ? lda : k), A_row_major);
viennacl::matrix_base<NumericT> matB(B, viennacl::MAIN_MEMORY,
B_size1, size_type(offB_row), difference_type(incB_row), size_type(B_row_major ? k : ldb),
B_size2, size_type(offB_col), difference_type(incB_col), size_type(B_row_major ? ldb : n), B_row_major);
viennacl::matrix_base<NumericT> matC(C, viennacl::MAIN_MEMORY,
size_type(m), size_type(offC_row), difference_type(incC_row), size_type(C_row_major ? m : ldc),
size_type(n), size_type(offC_col), difference_type(incC_col), size_type(C_row_major ? ldc : n), C_row_major);
detail::gemm_dispatch(alpha, matA, transA, matB, transB, beta, matC);
return ViennaCLSuccess;
}
// Helper rotation function.
mat3 Transform::rotate(const float degrees, const vec3& axis) {
// YOUR CODE FOR HW1 HERE
float rad = glm::radians(degrees);
mat3 matA(
1.0, 0.0, 0.0,
0.0, 1.0, 0.0,
0.0, 0.0, 1.0
);
mat3 matB(
axis.x*axis.x, axis.x*axis.y, axis.x*axis.z,
axis.x*axis.y, axis.y*axis.y, axis.y*axis.z,
axis.x*axis.z, axis.y*axis.z, axis.z*axis.z
);
mat3 matC(
0.0, axis.z, -axis.y,
-axis.z, 0.0, axis.x,
axis.y, -axis.x, 0.0
);
mat3 rot = cos(rad) * matA + (1-cos(rad))*matB + sin(rad)*matC;
// You will change this return call
return rot;
}
ViennaCLStatus ViennaCLOpenCLgemm_impl(ViennaCLBackend backend,
ViennaCLOrder orderA, ViennaCLTranspose transA,
ViennaCLOrder orderB, ViennaCLTranspose transB,
ViennaCLOrder orderC,
ViennaCLInt m, ViennaCLInt n, ViennaCLInt k,
NumericT alpha,
cl_mem A, ViennaCLInt offA_row, ViennaCLInt offA_col, ViennaCLInt incA_row, ViennaCLInt incA_col, ViennaCLInt lda,
cl_mem B, ViennaCLInt offB_row, ViennaCLInt offB_col, ViennaCLInt incB_row, ViennaCLInt incB_col, ViennaCLInt ldb,
NumericT beta,
cl_mem C, ViennaCLInt offC_row, ViennaCLInt offC_col, ViennaCLInt incC_row, ViennaCLInt incC_col, ViennaCLInt ldc)
{
ViennaCLInt A_size1 = (transA == ViennaCLTrans) ? k : m;
ViennaCLInt A_size2 = (transA == ViennaCLTrans) ? m : k;
ViennaCLInt B_size1 = (transB == ViennaCLTrans) ? n : k;
ViennaCLInt B_size2 = (transB == ViennaCLTrans) ? k : n;
bool A_row_major = (orderA == ViennaCLRowMajor);
bool B_row_major = (orderB == ViennaCLRowMajor);
bool C_row_major = (orderC == ViennaCLRowMajor);
viennacl::matrix_base<NumericT> matA(A, viennacl::ocl::get_context(backend->opencl_backend.context_id),
A_size1, offA_row, incA_row, A_row_major ? m : lda,
A_size2, offA_col, incA_col, A_row_major ? lda : k, A_row_major);
viennacl::matrix_base<NumericT> matB(B, viennacl::ocl::get_context(backend->opencl_backend.context_id),
B_size1, offB_row, incB_row, B_row_major ? k : ldb,
B_size2, offB_col, incB_col, B_row_major ? ldb : n, B_row_major);
viennacl::matrix_base<NumericT> matC(C, viennacl::ocl::get_context(backend->opencl_backend.context_id),
m, offC_row, incC_row, C_row_major ? m : ldc,
n, offC_col, incC_col, C_row_major ? ldc : n, C_row_major);
detail::gemm_dispatch(alpha, matA, transA, matB, transB, beta, matC);
return ViennaCLSuccess;
}
int main(int argc, char** argv)
{
ros::init(argc, argv, "laserMapping");
ros::NodeHandle nh;
ros::Subscriber subLaserCloudLast2 = nh.subscribe<sensor_msgs::PointCloud2>
("/laser_cloud_last_2", 2, laserCloudLastHandler);
ros::Subscriber subLaserOdometry = nh.subscribe<nav_msgs::Odometry>
("/cam_to_init", 5, laserOdometryHandler);
ros::Publisher pubLaserCloudSurround = nh.advertise<sensor_msgs::PointCloud2>
("/laser_cloud_surround", 1);
//ros::Publisher pub1 = nh.advertise<sensor_msgs::PointCloud2> ("/pc1", 1);
//ros::Publisher pub2 = nh.advertise<sensor_msgs::PointCloud2> ("/pc2", 1);
//ros::Publisher pub3 = nh.advertise<sensor_msgs::PointCloud2> ("/pc3", 1);
//ros::Publisher pub4 = nh.advertise<sensor_msgs::PointCloud2> ("/pc4", 1);
ros::Publisher pubOdomBefMapped = nh.advertise<nav_msgs::Odometry> ("/bef_mapped_to_init_2", 5);
nav_msgs::Odometry odomBefMapped;
odomBefMapped.header.frame_id = "/camera_init_2";
odomBefMapped.child_frame_id = "/bef_mapped";
ros::Publisher pubOdomAftMapped = nh.advertise<nav_msgs::Odometry> ("/aft_mapped_to_init_2", 5);
nav_msgs::Odometry odomAftMapped;
odomAftMapped.header.frame_id = "/camera_init_2";
odomAftMapped.child_frame_id = "/aft_mapped";
std::vector<int> pointSearchInd;
std::vector<float> pointSearchSqDis;
pcl::PointXYZHSV pointOri, pointSel, pointProj, coeff;
pcl::PointXYZI pointSurround;
cv::Mat matA0(5, 3, CV_32F, cv::Scalar::all(0));
cv::Mat matB0(5, 1, CV_32F, cv::Scalar::all(-1));
cv::Mat matX0(3, 1, CV_32F, cv::Scalar::all(0));
cv::Mat matA1(3, 3, CV_32F, cv::Scalar::all(0));
cv::Mat matD1(1, 3, CV_32F, cv::Scalar::all(0));
cv::Mat matV1(3, 3, CV_32F, cv::Scalar::all(0));
for (int i = 0; i < laserCloudNum; i++) {
laserCloudArray[i].reset(new pcl::PointCloud<pcl::PointXYZHSV>());
}
bool status = ros::ok();
while (status) {
ros::spinOnce();
if (newLaserCloudLast && newLaserOdometry && fabs(timeLaserCloudLast - timeLaserOdometry) < 0.005) {
newLaserCloudLast = false;
newLaserOdometry = false;
transformAssociateToMap();
pcl::PointXYZHSV pointOnZAxis;
pointOnZAxis.x = 0.0;
pointOnZAxis.y = 0.0;
pointOnZAxis.z = 10.0;
pointAssociateToMap(&pointOnZAxis, &pointOnZAxis);
int centerCubeI = int((transformTobeMapped[3] + 10.0) / 20.0) + laserCloudCenWidth;
int centerCubeJ = int((transformTobeMapped[4] + 10.0) / 20.0) + laserCloudCenHeight;
int centerCubeK = int((transformTobeMapped[5] + 10.0) / 20.0) + laserCloudCenDepth;
if (transformTobeMapped[3] + 10.0 < 0) centerCubeI--;
if (transformTobeMapped[4] + 10.0 < 0) centerCubeJ--;
if (transformTobeMapped[5] + 10.0 < 0) centerCubeK--;
if (centerCubeI < 0 || centerCubeI >= laserCloudWidth ||
centerCubeJ < 0 || centerCubeJ >= laserCloudHeight ||
centerCubeK < 0 || centerCubeK >= laserCloudDepth) {
transformUpdate();
continue;
}
int laserCloudValidNum = 0;
int laserCloudSurroundNum = 0;
for (int i = centerCubeI - 1; i <= centerCubeI + 1; i++) {
for (int j = centerCubeJ - 1; j <= centerCubeJ + 1; j++) {
for (int k = centerCubeK - 1; k <= centerCubeK + 1; k++) {
if (i >= 0 && i < laserCloudWidth &&
j >= 0 && j < laserCloudHeight &&
k >= 0 && k < laserCloudDepth) {
float centerX = 20.0 * (i - laserCloudCenWidth);
float centerY = 20.0 * (j - laserCloudCenHeight);
float centerZ = 20.0 * (k - laserCloudCenDepth);
bool isInLaserFOV = false;
for (int ii = -1; ii <= 1; ii += 2) {
for (int jj = -1; jj <= 1; jj += 2) {
for (int kk = -1; kk <= 1; kk += 2) {
float cornerX = centerX + 10.0 * ii;
float cornerY = centerY + 10.0 * jj;
float cornerZ = centerZ + 10.0 * kk;
float check = 100.0
+ (transformTobeMapped[3] - cornerX) * (transformTobeMapped[3] - cornerX)
+ (transformTobeMapped[4] - cornerY) * (transformTobeMapped[4] - cornerY)
+ (transformTobeMapped[5] - cornerZ) * (transformTobeMapped[5] - cornerZ)
- (pointOnZAxis.x - cornerX) * (pointOnZAxis.x - cornerX)
- (pointOnZAxis.y - cornerY) * (pointOnZAxis.y - cornerY)
- (pointOnZAxis.z - cornerZ) * (pointOnZAxis.z - cornerZ);
if (check > 0) {
isInLaserFOV = true;
}
}
}
}
if (isInLaserFOV) {
laserCloudValidInd[laserCloudValidNum] = i + laserCloudWidth * j
+ laserCloudWidth * laserCloudHeight * k;
laserCloudValidNum++;
}
laserCloudSurroundInd[laserCloudSurroundNum] = i + laserCloudWidth * j
+ laserCloudWidth * laserCloudHeight * k;
laserCloudSurroundNum++;
}
}
}
}
laserCloudFromMap->clear();
for (int i = 0; i < laserCloudValidNum; i++) {
*laserCloudFromMap += *laserCloudArray[laserCloudValidInd[i]];
}
int laserCloudFromMapNum = laserCloudFromMap->points.size();
laserCloudCornerFromMap->clear();
laserCloudSurfFromMap->clear();
for (int i = 0; i < laserCloudFromMapNum; i++) {
if (fabs(laserCloudFromMap->points[i].v - 2) < 0.005 ||
fabs(laserCloudFromMap->points[i].v - 1) < 0.005) {
laserCloudCornerFromMap->push_back(laserCloudFromMap->points[i]);
} else {
laserCloudSurfFromMap->push_back(laserCloudFromMap->points[i]);
}
}
laserCloudCorner->clear();
laserCloudSurf->clear();
int laserCloudLastNum = laserCloudLast->points.size();
for (int i = 0; i < laserCloudLastNum; i++) {
if (fabs(laserCloudLast->points[i].v - 2) < 0.005 ||
fabs(laserCloudLast->points[i].v - 1) < 0.005) {
laserCloudCorner->push_back(laserCloudLast->points[i]);
} else {
laserCloudSurf->push_back(laserCloudLast->points[i]);
}
}
laserCloudCorner2->clear();
pcl::VoxelGrid<pcl::PointXYZHSV> downSizeFilter;
downSizeFilter.setInputCloud(laserCloudCorner);
downSizeFilter.setLeafSize(0.05, 0.05, 0.05);
downSizeFilter.filter(*laserCloudCorner2);
laserCloudSurf2->clear();
downSizeFilter.setInputCloud(laserCloudSurf);
downSizeFilter.setLeafSize(0.1, 0.1, 0.1);
downSizeFilter.filter(*laserCloudSurf2);
laserCloudLast->clear();
*laserCloudLast = *laserCloudCorner2 + *laserCloudSurf2;
laserCloudLastNum = laserCloudLast->points.size();
if (laserCloudSurfFromMap->points.size() > 500) {
kdtreeCornerFromMap->setInputCloud(laserCloudCornerFromMap);
kdtreeSurfFromMap->setInputCloud(laserCloudSurfFromMap);
for (int iterCount = 0; iterCount < 10; iterCount++) {
laserCloudOri->clear();
//laserCloudSel->clear();
//laserCloudCorr->clear();
//laserCloudProj->clear();
coeffSel->clear();
for (int i = 0; i < laserCloudLastNum; i++) {
if (fabs(laserCloudLast->points[i].x > 0.3) || fabs(laserCloudLast->points[i].y > 0.3) ||
fabs(laserCloudLast->points[i].z > 0.3)) {
pointOri = laserCloudLast->points[i];
pointAssociateToMap(&pointOri, &pointSel);
if (fabs(pointOri.v) < 0.05 || fabs(pointOri.v + 1) < 0.05) {
kdtreeSurfFromMap->nearestKSearch(pointSel, 5, pointSearchInd, pointSearchSqDis);
if (pointSearchSqDis[4] < 1.0) {
for (int j = 0; j < 5; j++) {
matA0.at<float>(j, 0) = laserCloudSurfFromMap->points[pointSearchInd[j]].x;
matA0.at<float>(j, 1) = laserCloudSurfFromMap->points[pointSearchInd[j]].y;
matA0.at<float>(j, 2) = laserCloudSurfFromMap->points[pointSearchInd[j]].z;
}
cv::solve(matA0, matB0, matX0, cv::DECOMP_QR);
float pa = matX0.at<float>(0, 0);
float pb = matX0.at<float>(1, 0);
float pc = matX0.at<float>(2, 0);
float pd = 1;
float ps = sqrt(pa * pa + pb * pb + pc * pc);
pa /= ps;
pb /= ps;
pc /= ps;
pd /= ps;
bool planeValid = true;
for (int j = 0; j < 5; j++) {
if (fabs(pa * laserCloudSurfFromMap->points[pointSearchInd[j]].x +
pb * laserCloudSurfFromMap->points[pointSearchInd[j]].y +
pc * laserCloudSurfFromMap->points[pointSearchInd[j]].z + pd) > 0.05) {
planeValid = false;
break;
}
}
if (planeValid) {
float pd2 = pa * pointSel.x + pb * pointSel.y + pc * pointSel.z + pd;
pointProj = pointSel;
pointProj.x -= pa * pd2;
pointProj.y -= pb * pd2;
pointProj.z -= pc * pd2;
float s = 1;
if (iterCount >= 6) {
s = 1 - 8 * fabs(pd2) / sqrt(sqrt(pointSel.x * pointSel.x
+ pointSel.y * pointSel.y + pointSel.z * pointSel.z));
}
coeff.x = s * pa;
coeff.y = s * pb;
coeff.z = s * pc;
coeff.h = s * pd2;
if (s > 0.2) {
laserCloudOri->push_back(pointOri);
//laserCloudSel->push_back(pointSel);
//laserCloudProj->push_back(pointProj);
//laserCloudCorr->push_back(laserCloudSurfFromMap->points[pointSearchInd[0]]);
//laserCloudCorr->push_back(laserCloudSurfFromMap->points[pointSearchInd[1]]);
//laserCloudCorr->push_back(laserCloudSurfFromMap->points[pointSearchInd[2]]);
//laserCloudCorr->push_back(laserCloudSurfFromMap->points[pointSearchInd[3]]);
//laserCloudCorr->push_back(laserCloudSurfFromMap->points[pointSearchInd[4]]);
coeffSel->push_back(coeff);
}
}
}
} else {
kdtreeCornerFromMap->nearestKSearch(pointSel, 5, pointSearchInd, pointSearchSqDis);
if (pointSearchSqDis[4] < 1.0) {
float cx = 0;
float cy = 0;
float cz = 0;
for (int j = 0; j < 5; j++) {
cx += laserCloudCornerFromMap->points[pointSearchInd[j]].x;
cy += laserCloudCornerFromMap->points[pointSearchInd[j]].y;
cz += laserCloudCornerFromMap->points[pointSearchInd[j]].z;
}
cx /= 5;
cy /= 5;
cz /= 5;
float a11 = 0;
float a12 = 0;
float a13 = 0;
float a22 = 0;
float a23 = 0;
float a33 = 0;
for (int j = 0; j < 5; j++) {
float ax = laserCloudCornerFromMap->points[pointSearchInd[j]].x - cx;
float ay = laserCloudCornerFromMap->points[pointSearchInd[j]].y - cy;
float az = laserCloudCornerFromMap->points[pointSearchInd[j]].z - cz;
a11 += ax * ax;
a12 += ax * ay;
a13 += ax * az;
a22 += ay * ay;
a23 += ay * az;
a33 += az * az;
}
a11 /= 5;
a12 /= 5;
a13 /= 5;
a22 /= 5;
a23 /= 5;
a33 /= 5;
matA1.at<float>(0, 0) = a11;
matA1.at<float>(0, 1) = a12;
matA1.at<float>(0, 2) = a13;
matA1.at<float>(1, 0) = a12;
matA1.at<float>(1, 1) = a22;
matA1.at<float>(1, 2) = a23;
matA1.at<float>(2, 0) = a13;
matA1.at<float>(2, 1) = a23;
matA1.at<float>(2, 2) = a33;
cv::eigen(matA1, matD1, matV1);
if (matD1.at<float>(0, 0) > 3 * matD1.at<float>(0, 1)) {
float x0 = pointSel.x;
float y0 = pointSel.y;
float z0 = pointSel.z;
float x1 = cx + 0.1 * matV1.at<float>(0, 0);
float y1 = cy + 0.1 * matV1.at<float>(0, 1);
float z1 = cz + 0.1 * matV1.at<float>(0, 2);
float x2 = cx - 0.1 * matV1.at<float>(0, 0);
float y2 = cy - 0.1 * matV1.at<float>(0, 1);
float z2 = cz - 0.1 * matV1.at<float>(0, 2);
float a012 = sqrt(((x0 - x1)*(y0 - y2) - (x0 - x2)*(y0 - y1))
* ((x0 - x1)*(y0 - y2) - (x0 - x2)*(y0 - y1))
+ ((x0 - x1)*(z0 - z2) - (x0 - x2)*(z0 - z1))
* ((x0 - x1)*(z0 - z2) - (x0 - x2)*(z0 - z1))
+ ((y0 - y1)*(z0 - z2) - (y0 - y2)*(z0 - z1))
* ((y0 - y1)*(z0 - z2) - (y0 - y2)*(z0 - z1)));
float l12 = sqrt((x1 - x2)*(x1 - x2) + (y1 - y2)*(y1 - y2) + (z1 - z2)*(z1 - z2));
float la = ((y1 - y2)*((x0 - x1)*(y0 - y2) - (x0 - x2)*(y0 - y1))
+ (z1 - z2)*((x0 - x1)*(z0 - z2) - (x0 - x2)*(z0 - z1))) / a012 / l12;
float lb = -((x1 - x2)*((x0 - x1)*(y0 - y2) - (x0 - x2)*(y0 - y1))
- (z1 - z2)*((y0 - y1)*(z0 - z2) - (y0 - y2)*(z0 - z1))) / a012 / l12;
float lc = -((x1 - x2)*((x0 - x1)*(z0 - z2) - (x0 - x2)*(z0 - z1))
+ (y1 - y2)*((y0 - y1)*(z0 - z2) - (y0 - y2)*(z0 - z1))) / a012 / l12;
float ld2 = a012 / l12;
pointProj = pointSel;
pointProj.x -= la * ld2;
pointProj.y -= lb * ld2;
pointProj.z -= lc * ld2;
float s = 2 * (1 - 8 * fabs(ld2));
coeff.x = s * la;
coeff.y = s * lb;
coeff.z = s * lc;
coeff.h = s * ld2;
if (s > 0.4) {
laserCloudOri->push_back(pointOri);
//laserCloudSel->push_back(pointSel);
//laserCloudProj->push_back(pointProj);
//laserCloudCorr->push_back(laserCloudCornerFromMap->points[pointSearchInd[0]]);
//laserCloudCorr->push_back(laserCloudCornerFromMap->points[pointSearchInd[1]]);
//laserCloudCorr->push_back(laserCloudCornerFromMap->points[pointSearchInd[2]]);
//laserCloudCorr->push_back(laserCloudCornerFromMap->points[pointSearchInd[3]]);
//laserCloudCorr->push_back(laserCloudCornerFromMap->points[pointSearchInd[4]]);
coeffSel->push_back(coeff);
}
}
}
}
}
}
int laserCloudSelNum = laserCloudOri->points.size();
float srx = sin(transformTobeMapped[0]);
float crx = cos(transformTobeMapped[0]);
float sry = sin(transformTobeMapped[1]);
float cry = cos(transformTobeMapped[1]);
float srz = sin(transformTobeMapped[2]);
float crz = cos(transformTobeMapped[2]);
if (laserCloudSelNum < 50) {
continue;
}
cv::Mat matA(laserCloudSelNum, 6, CV_32F, cv::Scalar::all(0));
cv::Mat matAt(6, laserCloudSelNum, CV_32F, cv::Scalar::all(0));
cv::Mat matAtA(6, 6, CV_32F, cv::Scalar::all(0));
cv::Mat matB(laserCloudSelNum, 1, CV_32F, cv::Scalar::all(0));
cv::Mat matAtB(6, 1, CV_32F, cv::Scalar::all(0));
cv::Mat matX(6, 1, CV_32F, cv::Scalar::all(0));
for (int i = 0; i < laserCloudSelNum; i++) {
pointOri = laserCloudOri->points[i];
coeff = coeffSel->points[i];
float arx = (crx*sry*srz*pointOri.x + crx*crz*sry*pointOri.y - srx*sry*pointOri.z) * coeff.x
+ (-srx*srz*pointOri.x - crz*srx*pointOri.y - crx*pointOri.z) * coeff.y
+ (crx*cry*srz*pointOri.x + crx*cry*crz*pointOri.y - cry*srx*pointOri.z) * coeff.z;
float ary = ((cry*srx*srz - crz*sry)*pointOri.x
+ (sry*srz + cry*crz*srx)*pointOri.y + crx*cry*pointOri.z) * coeff.x
+ ((-cry*crz - srx*sry*srz)*pointOri.x
+ (cry*srz - crz*srx*sry)*pointOri.y - crx*sry*pointOri.z) * coeff.z;
float arz = ((crz*srx*sry - cry*srz)*pointOri.x + (-cry*crz - srx*sry*srz)*pointOri.y)*coeff.x
+ (crx*crz*pointOri.x - crx*srz*pointOri.y) * coeff.y
+ ((sry*srz + cry*crz*srx)*pointOri.x + (crz*sry - cry*srx*srz)*pointOri.y)*coeff.z;
matA.at<float>(i, 0) = arx;
matA.at<float>(i, 1) = ary;
matA.at<float>(i, 2) = arz;
matA.at<float>(i, 3) = coeff.x;
matA.at<float>(i, 4) = coeff.y;
matA.at<float>(i, 5) = coeff.z;
matB.at<float>(i, 0) = -coeff.h;
}
cv::transpose(matA, matAt);
matAtA = matAt * matA;
matAtB = matAt * matB;
cv::solve(matAtA, matAtB, matX, cv::DECOMP_QR);
if (fabs(matX.at<float>(0, 0)) < 0.5 &&
fabs(matX.at<float>(1, 0)) < 0.5 &&
fabs(matX.at<float>(2, 0)) < 0.5 &&
fabs(matX.at<float>(3, 0)) < 1 &&
fabs(matX.at<float>(4, 0)) < 1 &&
fabs(matX.at<float>(5, 0)) < 1) {
transformTobeMapped[0] += matX.at<float>(0, 0);
transformTobeMapped[1] += matX.at<float>(1, 0);
transformTobeMapped[2] += matX.at<float>(2, 0);
transformTobeMapped[3] += matX.at<float>(3, 0);
transformTobeMapped[4] += matX.at<float>(4, 0);
transformTobeMapped[5] += matX.at<float>(5, 0);
} else {
//ROS_INFO ("Mapping update out of bound");
}
}
}
transformUpdate();
for (int i = 0; i < laserCloudLastNum; i++) {
if (fabs(laserCloudLast->points[i].x) > 0.3 || fabs(laserCloudLast->points[i].y) > 0.3 ||
fabs(laserCloudLast->points[i].z) > 0.3) {
pointAssociateToMap(&laserCloudLast->points[i], &pointSel);
int cubeI = int((pointSel.x + 10.0) / 20.0) + laserCloudCenWidth;
int cubeJ = int((pointSel.y + 10.0) / 20.0) + laserCloudCenHeight;
int cubeK = int((pointSel.z + 10.0) / 20.0) + laserCloudCenDepth;
if (pointSel.x + 10.0 < 0) cubeI--;
if (pointSel.y + 10.0 < 0) cubeJ--;
if (pointSel.z + 10.0 < 0) cubeK--;
int cubeInd = cubeI + laserCloudWidth * cubeJ + laserCloudWidth * laserCloudHeight * cubeK;
laserCloudArray[cubeInd]->push_back(pointSel);
}
}
for (int i = 0; i < laserCloudValidNum; i++) {
laserCloudCorner->clear();
laserCloudSurf->clear();
pcl::PointCloud<pcl::PointXYZHSV>::Ptr laserCloudCubePointer =
laserCloudArray[laserCloudValidInd[i]];
int laserCloudCubeNum = laserCloudCubePointer->points.size();
for (int j = 0; j < laserCloudCubeNum; j++) {
if (fabs(laserCloudCubePointer->points[j].v - 2) < 0.005 ||
fabs(laserCloudCubePointer->points[j].v - 1) < 0.005) {
laserCloudCorner->push_back(laserCloudCubePointer->points[j]);
} else {
laserCloudSurf->push_back(laserCloudCubePointer->points[j]);
}
}
laserCloudCorner2->clear();
pcl::VoxelGrid<pcl::PointXYZHSV> downSizeFilter;
downSizeFilter.setInputCloud(laserCloudCorner);
downSizeFilter.setLeafSize(0.05, 0.05, 0.05);
downSizeFilter.filter(*laserCloudCorner2);
laserCloudSurf2->clear();
downSizeFilter.setInputCloud(laserCloudSurf);
downSizeFilter.setLeafSize(0.1, 0.1, 0.1);
downSizeFilter.filter(*laserCloudSurf2);
laserCloudCubePointer->clear();
*laserCloudCubePointer = *laserCloudCorner2 + *laserCloudSurf2;
}
laserCloudSurround->clear();
for (int i = 0; i < laserCloudSurroundNum; i++) {
pcl::PointCloud<pcl::PointXYZHSV>::Ptr laserCloudCubePointer =
laserCloudArray[laserCloudSurroundInd[i]];
int laserCloudCubeNum = laserCloudCubePointer->points.size();
for (int j = 0; j < laserCloudCubeNum; j++) {
pointSurround.x = laserCloudCubePointer->points[j].x;
pointSurround.y = laserCloudCubePointer->points[j].y;
pointSurround.z = laserCloudCubePointer->points[j].z;
pointSurround.intensity = laserCloudCubePointer->points[j].h;
laserCloudSurround->push_back(pointSurround);
}
}
sensor_msgs::PointCloud2 laserCloudSurround2;
pcl::toROSMsg(*laserCloudSurround, laserCloudSurround2);
laserCloudSurround2.header.stamp = ros::Time().fromSec(timeLaserCloudLast);
laserCloudSurround2.header.frame_id = "/camera_init_2";
pubLaserCloudSurround.publish(laserCloudSurround2);
geometry_msgs::Quaternion geoQuat = tf::createQuaternionMsgFromRollPitchYaw
(transformBefMapped[2], -transformBefMapped[0], -transformBefMapped[1]);
odomBefMapped.header.stamp = ros::Time().fromSec(timeLaserCloudLast);
odomBefMapped.pose.pose.orientation.x = -geoQuat.y;
odomBefMapped.pose.pose.orientation.y = -geoQuat.z;
odomBefMapped.pose.pose.orientation.z = geoQuat.x;
odomBefMapped.pose.pose.orientation.w = geoQuat.w;
odomBefMapped.pose.pose.position.x = transformBefMapped[3];
odomBefMapped.pose.pose.position.y = transformBefMapped[4];
odomBefMapped.pose.pose.position.z = transformBefMapped[5];
pubOdomBefMapped.publish(odomBefMapped);
geoQuat = tf::createQuaternionMsgFromRollPitchYaw
(transformAftMapped[2], -transformAftMapped[0], -transformAftMapped[1]);
odomAftMapped.header.stamp = ros::Time().fromSec(timeLaserCloudLast);
odomAftMapped.pose.pose.orientation.x = -geoQuat.y;
odomAftMapped.pose.pose.orientation.y = -geoQuat.z;
odomAftMapped.pose.pose.orientation.z = geoQuat.x;
odomAftMapped.pose.pose.orientation.w = geoQuat.w;
odomAftMapped.pose.pose.position.x = transformAftMapped[3];
odomAftMapped.pose.pose.position.y = transformAftMapped[4];
odomAftMapped.pose.pose.position.z = transformAftMapped[5];
pubOdomAftMapped.publish(odomAftMapped);
/*sensor_msgs::PointCloud2 pc12;
pcl::toROSMsg(*laserCloudCornerFromMap, pc12);
pc12.header.stamp = ros::Time().fromSec(timeLaserCloudLast);
pc12.header.frame_id = "/camera_init_2";
pub1.publish(pc12);
sensor_msgs::PointCloud2 pc22;
pcl::toROSMsg(*laserCloudSel, pc22);
pc22.header.stamp = ros::Time().fromSec(timeLaserCloudLast);
pc22.header.frame_id = "/camera_init_2";
pub2.publish(pc22);
sensor_msgs::PointCloud2 pc32;
pcl::toROSMsg(*laserCloudCorr, pc32);
pc32.header.stamp = ros::Time().fromSec(timeLaserCloudLast);
pc32.header.frame_id = "/camera_init_2";
pub3.publish(pc32);
sensor_msgs::PointCloud2 pc42;
pcl::toROSMsg(*laserCloudProj, pc42);
pc42.header.stamp = ros::Time().fromSec(timeLaserCloudLast);
pc42.header.frame_id = "/camera_init_2";
pub4.publish(pc42);*/
}
status = ros::ok();
cv::waitKey(10);
}
return 0;
}
template<typename Scalar> void sparse_solvers(int rows, int cols)
{
double density = std::max(8./(rows*cols), 0.01);
typedef Matrix<Scalar,Dynamic,Dynamic> DenseMatrix;
typedef Matrix<Scalar,Dynamic,1> DenseVector;
// Scalar eps = 1e-6;
DenseVector vec1 = DenseVector::Random(rows);
std::vector<Vector2i> zeroCoords;
std::vector<Vector2i> nonzeroCoords;
// test triangular solver
{
DenseVector vec2 = vec1, vec3 = vec1;
SparseMatrix<Scalar> m2(rows, cols);
DenseMatrix refMat2 = DenseMatrix::Zero(rows, cols);
// lower - dense
initSparse<Scalar>(density, refMat2, m2, ForceNonZeroDiag|MakeLowerTriangular, &zeroCoords, &nonzeroCoords);
VERIFY_IS_APPROX(refMat2.template triangularView<Lower>().solve(vec2),
m2.template triangularView<Lower>().solve(vec3));
// upper - dense
initSparse<Scalar>(density, refMat2, m2, ForceNonZeroDiag|MakeUpperTriangular, &zeroCoords, &nonzeroCoords);
VERIFY_IS_APPROX(refMat2.template triangularView<Upper>().solve(vec2),
m2.template triangularView<Upper>().solve(vec3));
// lower - transpose
initSparse<Scalar>(density, refMat2, m2, ForceNonZeroDiag|MakeLowerTriangular, &zeroCoords, &nonzeroCoords);
VERIFY_IS_APPROX(refMat2.transpose().template triangularView<Upper>().solve(vec2),
m2.transpose().template triangularView<Upper>().solve(vec3));
// upper - transpose
initSparse<Scalar>(density, refMat2, m2, ForceNonZeroDiag|MakeUpperTriangular, &zeroCoords, &nonzeroCoords);
VERIFY_IS_APPROX(refMat2.transpose().template triangularView<Lower>().solve(vec2),
m2.transpose().template triangularView<Lower>().solve(vec3));
SparseMatrix<Scalar> matB(rows, rows);
DenseMatrix refMatB = DenseMatrix::Zero(rows, rows);
// lower - sparse
initSparse<Scalar>(density, refMat2, m2, ForceNonZeroDiag|MakeLowerTriangular);
initSparse<Scalar>(density, refMatB, matB);
refMat2.template triangularView<Lower>().solveInPlace(refMatB);
m2.template triangularView<Lower>().solveInPlace(matB);
VERIFY_IS_APPROX(matB.toDense(), refMatB);
// upper - sparse
initSparse<Scalar>(density, refMat2, m2, ForceNonZeroDiag|MakeUpperTriangular);
initSparse<Scalar>(density, refMatB, matB);
refMat2.template triangularView<Upper>().solveInPlace(refMatB);
m2.template triangularView<Upper>().solveInPlace(matB);
VERIFY_IS_APPROX(matB, refMatB);
// test deprecated API
initSparse<Scalar>(density, refMat2, m2, ForceNonZeroDiag|MakeLowerTriangular, &zeroCoords, &nonzeroCoords);
VERIFY_IS_APPROX(refMat2.template triangularView<Lower>().solve(vec2),
m2.template triangularView<Lower>().solve(vec3));
}
}
ViennaCLStatus ViennaCLHostgemm_impl(ViennaCLBackend /*backend*/,
ViennaCLOrder orderA, ViennaCLTranspose transA,
ViennaCLOrder orderB, ViennaCLTranspose transB,
ViennaCLOrder orderC,
ViennaCLInt m, ViennaCLInt n, ViennaCLInt k,
NumericT alpha,
NumericT *A, ViennaCLInt offA_row, ViennaCLInt offA_col, ViennaCLInt incA_row, ViennaCLInt incA_col, ViennaCLInt lda,
NumericT *B, ViennaCLInt offB_row, ViennaCLInt offB_col, ViennaCLInt incB_row, ViennaCLInt incB_col, ViennaCLInt ldb,
NumericT beta,
NumericT *C, ViennaCLInt offC_row, ViennaCLInt offC_col, ViennaCLInt incC_row, ViennaCLInt incC_col, ViennaCLInt ldc)
{
ViennaCLInt A_size1 = (transA == ViennaCLTrans) ? k : m;
ViennaCLInt A_size2 = (transA == ViennaCLTrans) ? m : k;
ViennaCLInt B_size1 = (transB == ViennaCLTrans) ? n : k;
ViennaCLInt B_size2 = (transB == ViennaCLTrans) ? k : n;
/////// A row-major
if (orderA == ViennaCLRowMajor && orderB == ViennaCLRowMajor && orderC == ViennaCLRowMajor)
{
viennacl::matrix_base<NumericT> matA(A, viennacl::MAIN_MEMORY,
A_size1, offA_row, incA_row, m,
A_size2, offA_col, incA_col, lda);
viennacl::matrix_base<NumericT> matB(B, viennacl::MAIN_MEMORY,
B_size1, offB_row, incB_row, k,
B_size2, offB_col, incB_col, ldb);
viennacl::matrix_base<NumericT> matC(C, viennacl::MAIN_MEMORY,
m, offC_row, incC_row, m,
n, offC_col, incC_col, ldc);
detail::gemm_dispatch(alpha, matA, transA, matB, transB, beta, matC);
}
else if (orderA == ViennaCLRowMajor && orderB == ViennaCLRowMajor && orderC == ViennaCLColumnMajor)
{
viennacl::matrix_base<NumericT> matA(A, viennacl::MAIN_MEMORY,
A_size1, offA_row, incA_row, m,
A_size2, offA_col, incA_col, lda);
viennacl::matrix_base<NumericT> matB(B, viennacl::MAIN_MEMORY,
B_size1, offB_row, incB_row, k,
B_size2, offB_col, incB_col, ldb);
viennacl::matrix_base<NumericT, viennacl::column_major> matC(C, viennacl::MAIN_MEMORY,
m, offC_row, incC_row, ldc,
n, offC_col, incC_col, n);
detail::gemm_dispatch(alpha, matA, transA, matB, transB, beta, matC);
}
else if (orderA == ViennaCLRowMajor && orderB == ViennaCLColumnMajor && orderC == ViennaCLRowMajor)
{
viennacl::matrix_base<NumericT> matA(A, viennacl::MAIN_MEMORY,
A_size1, offA_row, incA_row, m,
A_size2, offA_col, incA_col, lda);
viennacl::matrix_base<NumericT, viennacl::column_major> matB(B, viennacl::MAIN_MEMORY,
B_size1, offB_row, incB_row, ldb,
B_size2, offB_col, incB_col, n);
viennacl::matrix_base<NumericT> matC(C, viennacl::MAIN_MEMORY,
m, offC_row, incC_row, m,
n, offC_col, incC_col, ldc);
detail::gemm_dispatch(alpha, matA, transA, matB, transB, beta, matC);
}
else if (orderA == ViennaCLRowMajor && orderB == ViennaCLColumnMajor && orderC == ViennaCLColumnMajor)
{
viennacl::matrix_base<NumericT> matA(A, viennacl::MAIN_MEMORY,
A_size1, offA_row, incA_row, m,
A_size2, offA_col, incA_col, lda);
viennacl::matrix_base<NumericT, viennacl::column_major> matB(B, viennacl::MAIN_MEMORY,
B_size1, offB_row, incB_row, ldb,
B_size2, offB_col, incB_col, n);
viennacl::matrix_base<NumericT, viennacl::column_major> matC(C, viennacl::MAIN_MEMORY,
m, offC_row, incC_row, ldc,
n, offC_col, incC_col, n);
detail::gemm_dispatch(alpha, matA, transA, matB, transB, beta, matC);
}
/////// A column-major
else if (orderA == ViennaCLColumnMajor && orderB == ViennaCLRowMajor && orderC == ViennaCLRowMajor)
{
viennacl::matrix_base<NumericT, viennacl::column_major> matA(A, viennacl::MAIN_MEMORY,
A_size1, offA_row, incA_row, lda,
A_size2, offA_col, incA_col, k);
viennacl::matrix_base<NumericT> matB(B, viennacl::MAIN_MEMORY,
B_size1, offB_row, incB_row, k,
B_size2, offB_col, incB_col, ldb);
viennacl::matrix_base<NumericT> matC(C, viennacl::MAIN_MEMORY,
m, offC_row, incC_row, m,
n, offC_col, incC_col, ldc);
detail::gemm_dispatch(alpha, matA, transA, matB, transB, beta, matC);
}
else if (orderA == ViennaCLColumnMajor && orderB == ViennaCLRowMajor && orderC == ViennaCLColumnMajor)
{
viennacl::matrix_base<NumericT, viennacl::column_major> matA(A, viennacl::MAIN_MEMORY,
A_size1, offA_row, incA_row, lda,
A_size2, offA_col, incA_col, k);
viennacl::matrix_base<NumericT> matB(B, viennacl::MAIN_MEMORY,
B_size1, offB_row, incB_row, k,
B_size2, offB_col, incB_col, ldb);
viennacl::matrix_base<NumericT, viennacl::column_major> matC(C, viennacl::MAIN_MEMORY,
m, offC_row, incC_row, ldc,
n, offC_col, incC_col, n);
detail::gemm_dispatch(alpha, matA, transA, matB, transB, beta, matC);
}
else if (orderA == ViennaCLColumnMajor && orderB == ViennaCLColumnMajor && orderC == ViennaCLRowMajor)
{
viennacl::matrix_base<NumericT, viennacl::column_major> matA(A, viennacl::MAIN_MEMORY,
A_size1, offA_row, incA_row, lda,
A_size2, offA_col, incA_col, k);
viennacl::matrix_base<NumericT, viennacl::column_major> matB(B, viennacl::MAIN_MEMORY,
B_size1, offB_row, incB_row, ldb,
B_size2, offB_col, incB_col, n);
viennacl::matrix_base<NumericT> matC(C, viennacl::MAIN_MEMORY,
m, offC_row, incC_row, m,
n, offC_col, incC_col, ldc);
detail::gemm_dispatch(alpha, matA, transA, matB, transB, beta, matC);
}
else if (orderA == ViennaCLColumnMajor && orderB == ViennaCLColumnMajor && orderC == ViennaCLColumnMajor)
{
viennacl::matrix_base<NumericT, viennacl::column_major> matA(A, viennacl::MAIN_MEMORY,
A_size1, offA_row, incA_row, lda,
A_size2, offA_col, incA_col, k);
viennacl::matrix_base<NumericT, viennacl::column_major> matB(B, viennacl::MAIN_MEMORY,
B_size1, offB_row, incB_row, ldb,
B_size2, offB_col, incB_col, n);
viennacl::matrix_base<NumericT, viennacl::column_major> matC(C, viennacl::MAIN_MEMORY,
m, offC_row, incC_row, ldc,
n, offC_col, incC_col, n);
detail::gemm_dispatch(alpha, matA, transA, matB, transB, beta, matC);
}
return ViennaCLSuccess;
}