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;
}
void Input::computeProjectionMat()
{
if (m_outImg3d.empty())
throw Exception("3d image invalid");
std::vector<std::vector<float> > objPoints;
std::vector<std::vector<float> > imgPoints;
int count = 6;
// create point correspondences with random points
objPoints.reserve(count);
imgPoints.reserve(count);
int maxNumIters = count * 20;
int numIters = 0;
do {
if (numIters > maxNumIters)
throw Exception("could not compute projection matrix");
numIters++;
float val1 = rand() / (float)RAND_MAX;
float val2 = rand() / (float)RAND_MAX;
int x = (int)(val1 * m_outImg3d.cols);
int y = (int)(val2 * m_outImg3d.rows);
if (x < 0 || x >= m_outImg3d.cols ||
y < 0 || y >= m_outImg3d.rows)
continue;
const float* row = m_outImg3d.ptr<float>(y);
std::vector<float> imgPoint(3);
imgPoint[0] = (float)x;
imgPoint[1] = (float)y;
imgPoint[2] = 1.0f;
std::vector<float> objPoint(4);
objPoint[0] = row[x * 3 + 0];
objPoint[1] = row[x * 3 + 1];
objPoint[2] = row[x * 3 + 2];
objPoint[3] = 1.0f;
if (objPoint[2] > 0) {
objPoints.push_back(objPoint);
imgPoints.push_back(imgPoint);
}
} while ((int)objPoints.size() < count);
// reconstruction from given point correspondences
// (see http://de.wikipedia.org/wiki/Projektionsmatrix#Berechnung_der_Projektionsmatrix_aus_Punktkorrespondenzen)
// create Matrix A
cv::Mat matA(2 * count, 12, CV_32FC1);
for (int i = 0, j = 0; i < 2 * count; i+=2, j++) { // cols
// first row
matA.ptr<float>(i)[0] = objPoints[j][0];
matA.ptr<float>(i)[1] = objPoints[j][1];
matA.ptr<float>(i)[2] = objPoints[j][2];
matA.ptr<float>(i)[3] = objPoints[j][3];
matA.ptr<float>(i)[4] = matA.ptr<float>(i)[5] =
matA.ptr<float>(i)[6] = matA.ptr<float>(i)[7] = 0;
matA.ptr<float>(i)[8] = - imgPoints[j][0] * objPoints[j][0];
matA.ptr<float>(i)[9] = - imgPoints[j][0] * objPoints[j][1];
matA.ptr<float>(i)[10] = - imgPoints[j][0] * objPoints[j][2];
matA.ptr<float>(i)[11] = - imgPoints[j][0] * objPoints[j][3];
// second row
matA.ptr<float>(i+1)[0] = matA.ptr<float>(i+1)[1] =
matA.ptr<float>(i+1)[2] = matA.ptr<float>(i+1)[3] = 0;
matA.ptr<float>(i+1)[4] = objPoints[j][0];
matA.ptr<float>(i+1)[5] = objPoints[j][1];
matA.ptr<float>(i+1)[6] = objPoints[j][2];
matA.ptr<float>(i+1)[7] = objPoints[j][3];
matA.ptr<float>(i+1)[8] = - imgPoints[j][1] * objPoints[j][0];
matA.ptr<float>(i+1)[9] = - imgPoints[j][1] * objPoints[j][1];
matA.ptr<float>(i+1)[10] = - imgPoints[j][1] * objPoints[j][2];
matA.ptr<float>(i+1)[11] = - imgPoints[j][1] * objPoints[j][3];
}
// solve using SVD
cv::Mat w;
cv::Mat u;
cv::Mat vt;
cv::SVD mySVD;
mySVD.compute(matA, w, u, vt);
// create matrix
m_projMat = cv::Mat(3, 4, CV_32FC1);
for (int i = 0; i < vt.rows; i++) {
m_projMat.ptr<float>(0)[i] = vt.ptr<float>(vt.cols - 1)[i] /
vt.ptr<float>(vt.cols - 1)[vt.rows - 1];
}
// normalize projection matrix
float sum = m_projMat.ptr<float>(2)[0] +
m_projMat.ptr<float>(2)[1] +
m_projMat.ptr<float>(2)[2];
m_projMat = m_projMat * (1.0f / sum);
// decompose
cv::decomposeProjectionMatrix(m_projMat, m_matCamera, m_matRot, m_matTrans);
}
int main()
{
//Specify data directory
//const std::string dataDirectory = "/Users/ervislilaj/Desktop/data/SimudPuteh";
const std::string dataDirectory = "/Users/ervislilaj/Desktop/data/SmallCaveDownsampled";
//Calculate paths
const std::string outputDirectory = dataDirectory + "/output";
const std::string offFile = dataDirectory + "/model.off";
const std::string skeletonFile = dataDirectory + "/model.skel";
const std::string segmentationFile = dataDirectory + "/segmentation.seg";
const std::string segmentationFile2 = dataDirectory + "/segmentation2.seg";
//Read files
typedef std::vector<Triangle> TriangleList;
std::vector<Eigen::Vector3f> vertices;
std::vector<Eigen::Vector3f> newVertices /*(vertices.size()*2)*/;
TriangleList triangles;
std::vector<Eigen::Vector3f> border;
std::vector<CurveSkeleton::Vertex> skeletonVertices;
std::vector<Vertex> gang;
std::vector<Eigen::Vector3f> borderVertices;
std::vector<Eigen::Vector3f> intersectionVertexForTriangulation;
std::vector<IndexedTriangle> triIndices;
std::vector<IndexedTriangle> borderIndices;
std::vector<IndexedTriangle> labelIndices;
std::vector<IndexedTriangle> leerIndices;
std::vector<Eigen::Vector3f> verwe;
std::vector<int32_t> segmentation;
std::vector<int32_t> segmentation2;
std::vector<double> doubleSeg;
std::vector<int> labels;
//step & iteration parameter
int iter = 490;
double step = 0.001;
std::cout << "Loading mesh file..." << std::endl;
ReadOff(offFile, vertices, triangles, triIndices);
std::cout << "Loading skeleton..." << std::endl;
CurveSkeleton* skeleton = LoadCurveSkeleton(skeletonFile.c_str());
std::cout << "Loading segmentation..." << std::endl;
ReadSegmentation(segmentationFile2, segmentation2, skeleton);
//copy
std::vector<Vertex> vec2;
std::cout << "Calculating inverse correspondences..." << std::endl;
//Calculate inverse correspondences
std::vector<unsigned int> meshVertexCorrespondsTo(vertices.size());
int iVert = -1;
for (auto& vert : skeleton->vertices)
{
++iVert;
for (auto c : vert.correspondingOriginalVertices)
{
meshVertexCorrespondsTo[c] = iVert;
}
}
for (int i = 0; i < segmentation2.size(); ++i) {
doubleSeg.push_back(segmentation2[i]);
}
for (int i = 0; i < vertices.size(); ++i) {
Vertex* new_v = new Vertex();
new_v->p = vertices[i];
new_v->c = doubleSeg[meshVertexCorrespondsTo[i]] + 0.5;
newVertices.push_back(vertices[i]);
vec2.push_back(new_v[0]);
}
cgv::math::union_find ufTri((int)vertices.size());
borderIndices.clear();
labels.clear();
labelIndices.clear();
//set neighbors id
for (int i = 0; i < triIndices.size(); ++i)
{
for (int j = 0; j < triIndices.size(); ++j)
{
if(i==j);
else{
if (((triIndices[j].i[0] == triIndices[i].i[0]) && (triIndices[j].i[1] == triIndices[i].i[1])) ||
((triIndices[j].i[1] == triIndices[i].i[0]) && (triIndices[j].i[0] == triIndices[i].i[1])) ||
((triIndices[j].i[1] == triIndices[i].i[0]) && (triIndices[j].i[2] == triIndices[i].i[1])) ||
((triIndices[j].i[2] == triIndices[i].i[0]) && (triIndices[j].i[1] == triIndices[i].i[1])) ||
((triIndices[j].i[2] == triIndices[i].i[0]) && (triIndices[j].i[0] == triIndices[i].i[1])) ||
((triIndices[j].i[0] == triIndices[i].i[0]) && (triIndices[j].i[2] == triIndices[i].i[1])))
{
// neighbor 1
triIndices[i].n[0] = &triIndices[j];
}
if (((triIndices[j].i[0] == triIndices[i].i[1]) && (triIndices[j].i[1] == triIndices[i].i[2])) ||
((triIndices[j].i[1] == triIndices[i].i[1]) && (triIndices[j].i[0] == triIndices[i].i[2])) ||
((triIndices[j].i[1] == triIndices[i].i[1]) && (triIndices[j].i[2] == triIndices[i].i[2])) ||
((triIndices[j].i[2] == triIndices[i].i[1]) && (triIndices[j].i[1] == triIndices[i].i[2])) ||
((triIndices[j].i[2] == triIndices[i].i[1]) && (triIndices[j].i[0] == triIndices[i].i[2])) ||
((triIndices[j].i[0] == triIndices[i].i[1]) && (triIndices[j].i[2] == triIndices[i].i[2])))
{
// neighbor 2
triIndices[i].n[1] = &triIndices[j];
}
if (((triIndices[j].i[0] == triIndices[i].i[2]) && (triIndices[j].i[1] == triIndices[i].i[0])) ||
((triIndices[j].i[1] == triIndices[i].i[2]) && (triIndices[j].i[0] == triIndices[i].i[0])) ||
((triIndices[j].i[1] == triIndices[i].i[2]) && (triIndices[j].i[2] == triIndices[i].i[0])) ||
((triIndices[j].i[2] == triIndices[i].i[2]) && (triIndices[j].i[1] == triIndices[i].i[0])) ||
((triIndices[j].i[2] == triIndices[i].i[2]) && (triIndices[j].i[0] == triIndices[i].i[0])) ||
((triIndices[j].i[0] == triIndices[i].i[2]) && (triIndices[j].i[2] == triIndices[i].i[0])))
{
// neighbor 3
triIndices[i].n[2] = &triIndices[j];
}
}
}
}
/*
std::vector<IndexedTriangle> toShow;
for(int i = 0; i < triIndices.size(); i += 50)
{
toShow.push_back(triIndices[i]);
toShow.push_back(*triIndices[i].n[0]);
toShow.push_back(*triIndices[i].n[1]);
toShow.push_back(*triIndices[i].n[2]);
}
*/
borderIndices.clear();
labels.clear();
labelIndices.clear();
for (int i = 0; i < vec2.size(); ++i) //für jeden Vertex
{
for (int j = 0; j < triIndices.size(); ++j) //für jedes Dreieck
{
if ((i == triIndices[j].i[0] || i == triIndices[j].i[1] || i == triIndices[j].i[2]))
{
vec2[i].NeigborTri.push_back(j);
}
}
}
int nrOptimization = 0;
double tmpLengthIter = 0;
double lengthIter = 0;
bool firstTimeIter = false;
do
{
std::ofstream fout("iter/50iter" + std::to_string(nrOptimization) + ".xyz", std::ofstream::out);
if(!firstTimeIter)
tmpLengthIter = 99999999;
tmpLengthIter = lengthIter;
lengthIter = 0;
// 1 richtung
for (int i = 0; i < vec2.size(); ++i) //für jeden Vertex
{
double inzidenzEdgesLength = 0;
double tmp = 0;
int index = 0;
do {
Edge e;
tmp = inzidenzEdgesLength;
if (index == 0)
tmp = 10000;
inzidenzEdgesLength = 0;
for (int j = 0; j < vec2[i].NeigborTri.size(); ++j) //für jedes Dreieck
{
triIndices[vec2[i].NeigborTri[j]].visible = true;
if ((i == triIndices[vec2[i].NeigborTri[j]].i[0] || i == triIndices[vec2[i].NeigborTri[j]].i[1] || i == triIndices[vec2[i].NeigborTri[j]].i[2]))
{
//Kanten rechnen sowie deren länge
if (ComputePointsBetweenEdges_v1(vec2, triIndices[vec2[i].NeigborTri[j]], e))
{
e.calculateLength();
inzidenzEdgesLength += e.length;
}
}
}
index++;
vec2[i].c += step;
} while (tmp > inzidenzEdgesLength );
vec2[i].c -= step;
vec2[i].edgeLength = inzidenzEdgesLength;
}
// 2 richtung
for (int i = 0; i < vec2.size(); ++i) //für jeden Vertex
{
double inzidenzEdgesLength = 0;
double tmp = 0;
int index = 0;
do {
Edge e;
tmp = inzidenzEdgesLength;
if (index == 0)
tmp = vec2[i].edgeLength;
inzidenzEdgesLength = 0;
for (int j = 0; j < vec2[i].NeigborTri.size(); ++j) //für jedes Dreieck
{
triIndices[vec2[i].NeigborTri[j]].visible = true;
if ((i == triIndices[vec2[i].NeigborTri[j]].i[0] || i == triIndices[vec2[i].NeigborTri[j]].i[1] || i == triIndices[vec2[i].NeigborTri[j]].i[2]))
{
//Kanten rechnen sowie deren länge
if (ComputePointsBetweenEdges_v1(vec2, triIndices[vec2[i].NeigborTri[j]], e))
{
e.calculateLength();
inzidenzEdgesLength += e.length;
}
}
}
//edgeLengthBlock = inzidenzEdgesLength
index++;
vec2[i].c -= step;
lengthIter += inzidenzEdgesLength;
} while (tmp > inzidenzEdgesLength );
vec2[i].c += step;
}
for (int j = 0; j < triIndices.size(); ++j) //für jedes Dreieck
{
Edge e2;
if (ComputePointsBetweenEdges_v1(vec2, triIndices[j], e2)) {
fout << e2.a.x() << " " << e2.a.y() << " " << e2.a.z() << std::endl;
fout << e2.b.x() << " " << e2.b.y() << " " << e2.b.z() << std::endl;
}
}
nrOptimization++;
fout.close();
std::cout << nrOptimization << std::endl;
std::cout << lengthIter << std::endl;
} while ( (nrOptimization < iter ) && ((tmpLengthIter >= lengthIter) || (nrOptimization < 450)) );
/*End Optimization*/
borderVertices.clear();
for (int j = 0; j < triIndices.size(); ++j) //für jedes Dreieck
{
triIndices[j].iD = j;
Edge e2;
if (ComputePointsBetweenEdges_v1(vec2, triIndices[j], e2)) {
borderVertices.push_back(e2.a);
borderVertices.push_back(e2.b);
}
}
// übergang erkennen
for (int i = 0; i < triIndices.size(); ++i) {
auto v0 = vec2.begin() + (triIndices[i].i[0]);
auto v1 = vec2.begin() + (triIndices[i].i[1]);
auto v2 = vec2.begin() + (triIndices[i].i[2]);
double first = v0[0].c;
double second = v1[0].c;
double third = v2[0].c;
if ((first >= 0.f && second < 0.f) || (first < 0.f && second >= 0.f) || (third >= 0.f && second < 0.f) || (third < 0.f && second >= 0.f) || (third >= 0.f && first < 0.f) || (third < 0.f && first >= 0.f)) {
IndexedTriangle t;
t.i[0] = triIndices[i].i[0];
t.i[1] = triIndices[i].i[1];
t.i[2] = triIndices[i].i[2];
t.iD = triIndices[i].iD;
t.n[0] = triIndices[i].n[0];
t.n[1] = triIndices[i].n[1];
t.n[2] = triIndices[i].n[2];
borderIndices.push_back(t);
}
}
//schallen unite
for (int i = 0; i < borderIndices.size(); ++i)
for (int j = 0; j < borderIndices.size(); ++j)
{
if ((borderIndices[i].i[0] == borderIndices[j].i[0]) || (borderIndices[i].i[0] == borderIndices[j].i[1]) || (borderIndices[i].i[0] == borderIndices[j].i[2]))
{
ufTri.unite(i, j);
}
if ((borderIndices[i].i[1] == borderIndices[j].i[0]) || (borderIndices[i].i[1] == borderIndices[j].i[1]) || (borderIndices[i].i[1] == borderIndices[j].i[2]))
{
ufTri.unite(i, j);
}
if ((borderIndices[i].i[2] == borderIndices[j].i[0]) || (borderIndices[i].i[2] == borderIndices[j].i[1]) || (borderIndices[i].i[2] == borderIndices[j].i[2]))
{
ufTri.unite(i, j);
}
}
std::map<int, std::vector<IndexedTriangle>> labelmain;
// in LabelIndices 1 eiziger gang speichern
for (int i = 0; i < borderIndices.size(); ++i)
{
if (ufTri.num_in_set(ufTri.find(i)) <= 1);
else {
if (std::find(labels.begin(), labels.end(), ufTri.find(i)) != labels.end());
else {
labels.push_back(ufTri.find(i));
}
}
if (labels.size() != 0)
{
int key = ufTri.find(i);
labelmain[key].push_back(borderIndices[i]);
}
}
std::vector<IndexedTriangle> newTriangles;
int offset = (int)vec2.size();
for (int label : labels)
{
labelIndices = labelmain[label];
std::vector<Eigen::Vector3f> labelVertices;
std::vector<Eigen::Vector3f> halleVertices;
labelVertices.clear();
Edge labelEdge;
for (int i = 0; i < labelIndices.size(); ++i)
{
if (ComputePointsBetweenEdges_v1(vec2, labelIndices[i], labelEdge))
{
labelVertices.push_back(labelEdge.a);
labelVertices.push_back(labelEdge.b);
}
}
/*Plane Fitting with SVD */
Eigen::Vector3f vor_c(0, 0, 0);
Eigen::Vector3f c(0, 0, 0);
Eigen::MatrixXf matA(3, labelVertices.size());
// calculate c
for (int i = 0; i < labelVertices.size(); ++i)
{
vor_c += labelVertices[i];
}
c = vor_c / labelVertices.size();
//fill the Matrix
for (int i = 0; i < labelVertices.size(); i++)
{
float xB, yB, zB;
xB = labelVertices[i].x() - c.x();
yB = labelVertices[i].y() - c.y();
zB = labelVertices[i].z() - c.z();
matA.col(i) << xB, yB, zB;
}
Eigen::JacobiSVD<Eigen::MatrixXf> svd(matA, Eigen::ComputeThinU | Eigen::ComputeThinV);
Eigen::Vector3f planeNormal(0, 0, 0);
planeNormal = svd.matrixU().col(2);
float d = -(planeNormal.x()*c.x() + planeNormal.y()*c.y() + planeNormal.z()*c.z());
/*Begin Normal direction of the plane*/
std::vector<Eigen::Vector3f> halleVerticesSidePLane;
std::vector<double> disthalleVerticesSidePLane;
int indexHalleV;
Eigen::Vector3f halle_cVector;
//alle halle knoten hinzufügen
for(int i = 0; i < labelIndices.size(); ++i)
{
if(vec2[labelIndices[i].i[0]].c >= 0)
halleVerticesSidePLane.push_back(vec2[labelIndices[i].i[0]].p);
if(vec2[labelIndices[i].i[1]].c >= 0)
halleVerticesSidePLane.push_back(vec2[labelIndices[i].i[1]].p);
if(vec2[labelIndices[i].i[2]].c >= 0)
halleVerticesSidePLane.push_back(vec2[labelIndices[i].i[2]].p);
}
for(int i = 0; i < halleVerticesSidePLane.size(); ++i)
{
disthalleVerticesSidePLane.push_back( Dist2Plane(halleVerticesSidePLane[i], planeNormal, d));
}
indexHalleV = *std::max_element(disthalleVerticesSidePLane.begin(), disthalleVerticesSidePLane.end());
halle_cVector = halleVerticesSidePLane[indexHalleV] - c;
Eigen::Vector3f diffBetweenPlane_halle_positive = halle_cVector + planeNormal;
Eigen::Vector3f diffBetweenPlane_halle_negative = halle_cVector - planeNormal;
float lengthPositive =sqrt(diffBetweenPlane_halle_positive.x()* diffBetweenPlane_halle_positive.x() + diffBetweenPlane_halle_positive.y()* diffBetweenPlane_halle_positive.y() + diffBetweenPlane_halle_positive.z()* diffBetweenPlane_halle_positive.z() );
float lengthNegative =sqrt(diffBetweenPlane_halle_negative.x()* diffBetweenPlane_halle_negative.x() + diffBetweenPlane_halle_negative.y()* diffBetweenPlane_halle_negative.y() + diffBetweenPlane_halle_negative.z()* diffBetweenPlane_halle_negative.z() );
if (lengthPositive > lengthNegative)
planeNormal = -planeNormal;
else if(lengthPositive <= lengthNegative)
planeNormal = planeNormal;
// ax + bx + cx + d = 0 Plane equation
d = -(planeNormal.x()*c.x() + planeNormal.y()*c.y() + planeNormal.z()*c.z());
/*Begin computing Intersection Points*/
//Point which has the largest distance to c
std::vector<double> MaxDistanceToC;
double radiusMax;
for (int i = 0; i < labelVertices.size(); ++i)
{
MaxDistanceToC.push_back(Dist2Points(c, labelVertices[i]));
}
radiusMax = *std::max_element(MaxDistanceToC.begin(), MaxDistanceToC.end());
//Computing Intersection points
int succesor = offset - 1;
std::vector<Edge> intersectionEdges;
intersectionEdges.clear();
//Label Nachbarn hinzufügen. 8schritte 3^12
// First Triangle Hinzufügen die geschnitten wird und teil von lableindices ist
IndexedTriangle firstTri;
int firstTriID = 0;
bool firstTimeSearch = true;
for (int i = 0; i < labelIndices.size(); ++i){
Eigen::Vector3f aP, bP, cP, intersection1, intersection2;
int aI = 0, bI = 0, cI = 0;
aI = labelIndices[i].i[0];
bI = labelIndices[i].i[1];
cI = labelIndices[i].i[2];
aP = vec2[aI].p;
bP = vec2[bI].p;
cP = vec2[cI].p;
if((SegPlaneIntersection(aP, bP, intersection1, planeNormal, d) || SegPlaneIntersection(aP, cP, intersection2, planeNormal, d) ||
SegPlaneIntersection(bP, cP, intersection2, planeNormal, d)) && firstTimeSearch)
{
firstTri = triIndices[labelIndices[i].iD];
firstTimeSearch = false;
firstTriID = labelIndices[i].iD;
}
}
std::set<int> triIndicesHit ;
int mico = 0;
do{
for (int m = 0; m < 3 ; ++m)
{
Eigen::Vector3f aP, bP, cP, intersection1, intersection2;
IndexedTriangle t1,t2;
//indices
int aI = firstTri.n[m]->i[0];
int bI = firstTri.n[m]->i[1];
int cI = firstTri.n[m]->i[2];
aP = vec2[aI].p;
bP = vec2[bI].p;
cP = vec2[cI].p;
if((SegPlaneIntersection(aP, bP, intersection1, planeNormal, d) || SegPlaneIntersection(aP, cP, intersection2, planeNormal, d) ||
SegPlaneIntersection(bP, cP, intersection2, planeNormal, d)) && (triIndicesHit.find(firstTri.n[m]->iD) == triIndicesHit.end())){
triIndicesHit.insert(firstTri.n[m]->iD);
if (SegPlaneIntersection(aP, bP, intersection1, planeNormal, d) && SegPlaneIntersection(aP, cP, intersection2, planeNormal, d) && (Dist2Plane(aP, planeNormal, d) < 0))
{
newVertices.push_back(intersection1);
newVertices.push_back(intersection2);
t1.i[0] = aI;
t1.i[2] = succesor + 2;
t1.i[1] = succesor + 1;
t1.visible = true;
newTriangles.push_back(t1);
firstTri = *firstTri.n[m];
succesor++;
succesor++;
}
if (SegPlaneIntersection(bP, cP, intersection1, planeNormal, d) && SegPlaneIntersection(bP, aP, intersection2, planeNormal, d) && (Dist2Plane(bP, planeNormal, d) < 0))
{
newVertices.push_back(intersection1);
newVertices.push_back(intersection2);
t1.i[0] = bI;
t1.i[2] = succesor + 2;
t1.i[1] = succesor + 1;
t1.visible = true;
newTriangles.push_back(t1);
firstTri = *firstTri.n[m];
succesor++;
succesor++;
}
if (SegPlaneIntersection(cP, aP, intersection1, planeNormal, d) && SegPlaneIntersection(cP, bP, intersection2, planeNormal, d) && (Dist2Plane(cP, planeNormal, d) < 0))
{
newVertices.push_back(intersection1);
newVertices.push_back(intersection2);
t1.i[0] = cI;
t1.i[2] = succesor + 2;
t1.i[1] = succesor + 1;
t1.visible = true;
newTriangles.push_back(t1);
firstTri = *firstTri.n[m];
succesor++;
succesor++;
}
if (SegPlaneIntersection(aP, bP, intersection1, planeNormal, d) && SegPlaneIntersection(aP, cP, intersection2, planeNormal, d) && (Dist2Plane(bP, planeNormal, d) < 0) && (Dist2Plane(cP, planeNormal, d) < 0))
{
newVertices.push_back(intersection1);
newVertices.push_back(intersection2);
t1.i[0] = bI;
t1.i[2] = succesor + 1;
t1.i[1] = succesor + 2;
t2.i[0] = cI;
t2.i[2] = bI;
t2.i[1] = succesor + 2;
t1.visible = true;
t2.visible = true;
newTriangles.push_back(t1);
newTriangles.push_back(t2);
firstTri = *firstTri.n[m];
succesor++;
succesor++;
}
if (SegPlaneIntersection(bP, cP, intersection1, planeNormal, d) && SegPlaneIntersection(bP, aP, intersection2, planeNormal, d) && (Dist2Plane(cP, planeNormal, d) < 0) && (Dist2Plane(aP, planeNormal, d) < 0))
{
newVertices.push_back(intersection1);
newVertices.push_back(intersection2);
t1.i[0] = cI;
t1.i[2] = succesor + 1;
t1.i[1] = succesor + 2;
t2.i[0] = aI;
t2.i[2] = cI;
t2.i[1] = succesor + 2;
t1.visible = true;
t2.visible = true;
newTriangles.push_back(t1);
newTriangles.push_back(t2);
firstTri = *firstTri.n[m];
succesor++;
succesor++;
}
if (SegPlaneIntersection(cP, aP, intersection1, planeNormal, d) && SegPlaneIntersection(cP, bP, intersection2, planeNormal, d) && (Dist2Plane(aP, planeNormal, d) < 0) && (Dist2Plane(bP, planeNormal, d) < 0))
{
newVertices.push_back(intersection1);
newVertices.push_back(intersection2);
t1.i[0] = aI;
t1.i[2] = succesor + 1;
t1.i[1] = succesor + 2;
t2.i[0] = bI;
t2.i[2] = aI;
t2.i[1] = succesor + 2;
t1.visible = true;
t2.visible = true;
newTriangles.push_back(t1);
newTriangles.push_back(t2);
firstTri = *firstTri.n[m];
succesor++;
succesor++;
}
}
}
mico ++;
}while (mico < 1000000);
//nachbarn von Schnitt hinzufügen
std::set<int> extendedLabelIndices ;
for (int i = 0; i < triIndices.size(); ++i)
{
const bool is_in = triIndicesHit.find(triIndices[i].iD) != triIndicesHit.end();
if(is_in){
extendedLabelIndices.insert(triIndices[i].iD);
for(int j = 0; j < 3; ++j){
extendedLabelIndices.insert(triIndices[i].n[j]->iD);
for(int k = 0; k < 3; ++k){
extendedLabelIndices.insert(triIndices[i].n[j]->n[k]->iD);
for(int l = 0; l < 3; ++l){
extendedLabelIndices.insert(triIndices[i].n[j]->n[k]->n[l]->iD);
for(int f = 0; f < 3; ++f){
extendedLabelIndices.insert(triIndices[i].n[j]->n[k]->n[l]->n[f]->iD);
for(int s = 0; s < 3; ++s){
extendedLabelIndices.insert(triIndices[i].n[j]->n[k]->n[l]->n[f]->n[s]->iD);
for(int r = 0; r < 3; ++r){
extendedLabelIndices.insert(triIndices[i].n[j]->n[k]->n[l]->n[f]->n[s]->n[r]->iD);
}
}
}
}
}
}
}
}
/* for (int i = 0; i < triIndices.size(); ++i)
{
int a = triIndices[i].i[0],b = triIndices[i].i[1] ,c = triIndices[i].i[2] ;
if(( vec2[a].c >= 0 )&& ( vec2[b].c >= 0 ) && ( vec2[c].c >= 0 ))
{
triIndices[i].visible = true;
}
}*/
for (int i = 0; i < triIndices.size(); ++i)
{
//positions
Eigen::Vector3f aP, bP, cP, intersection1, intersection2;
//distance to middlepoint
float dista, distb, distc;
//indices
int aI = 0, bI = 0, cI = 0;
aI = triIndices[i].i[0];
bI = triIndices[i].i[1];
cI = triIndices[i].i[2];
aP = vec2[aI].p;
bP = vec2[bI].p;
cP = vec2[cI].p;
dista = Dist2Points(c, aP);
distb = Dist2Points(c, bP);
distc = Dist2Points(c, cP);
const bool is_in = extendedLabelIndices.find(triIndices[i].iD) != extendedLabelIndices.end();
if (is_in)
{
if ((Dist2Plane(aP, planeNormal, d) > 0) || (Dist2Plane(bP, planeNormal, d) > 0) || (Dist2Plane(cP, planeNormal, d) > 0))
{
triIndices[i].visible = false;
}
if (((Dist2Plane(aP, planeNormal, d) <= 0) || (Dist2Plane(bP, planeNormal, d) <= 0) || (Dist2Plane(cP, planeNormal, d) <= 0)) && ((Dist2Plane(aP, planeNormal, d) > 10) || (Dist2Plane(bP, planeNormal, d) > 10) || (Dist2Plane(cP, planeNormal, d) > 10)))
{
triIndices[i].visible = true;
}
}
}
//umbrella
Eigen::Vector3f vor_ca(0,0,0), ca(0, 0, 0);
int anz = (int)newVertices.size() - (offset);
for (int j = offset ; j < (int)newVertices.size() ; ++j)
{
vor_ca += newVertices[j];
}
ca = vor_c / anz;
newVertices.push_back(c);
for (int j = offset; j < newVertices.size()-2; j+=2)
{
IndexedTriangle t;
t.i[1] = j;
t.i[2] = j + 1;
t.i[0] = (int)newVertices.size() - 1;
newTriangles.push_back(t);
}
offset = (int)newVertices.size();
/*End computing Intersection Points*/
}
for(auto t : triIndices)
if(t.visible && (vec2[t.i[0]].c >= 0 || vec2[t.i[1]].c >= 0 || vec2[t.i[2]].c >= 0))
newTriangles.push_back(t);
WriteSegmentation(segmentationFile, segmentation2);
ReadSegmentation(segmentationFile, segmentation, skeleton);
//Test output
std::cout << "Writing output file..." << std::endl;
/*int colors[10][3] =
{
{ 166, 206, 227 },
{ 31, 120, 180 },
{ 251, 154, 153 },
{ 227, 26, 28 },
{ 253, 191, 111 },
{ 255, 127, 0 },
{ 202, 178, 214 },
{ 106, 61, 154 },
{ 255, 255, 153 },
{ 177, 89, 40 }
};*/
auto colorFunc = [&](int i, int& r, int& g, int& b)
{
if (doubleSeg[i] == -1)
{
//color for passages
r = 0; g = 175; b = 0;
}
else
if (doubleSeg[i] >= 0)
{
r = 166; g = 175; b = 227;
}
else
if (doubleSeg[i] == 0)
{
r = 0; g = 0; b = 175;
}
};
std::string segmentedMeshFile = outputDirectory + "/segmentedMesh.off";
WriteOff(segmentedMeshFile.c_str(), vertices, triIndices, [&](int i, int& r, int& g, int& b) {colorFunc(meshVertexCorrespondsTo[i], r, g, b); });
std::string segmentedMeshFile2 = outputDirectory + "/borderVertices.off";
WriteOff(segmentedMeshFile2.c_str(), vertices, borderIndices, [&](int i, int& r, int& g, int& b) {colorFunc(meshVertexCorrespondsTo[i], r, g, b); });
std::string segmentedMeshFile3 = outputDirectory + "/schalle.off";
WriteOff(segmentedMeshFile3.c_str(), vertices, borderIndices, [&](int i, int& r, int& g, int& b) {colorFunc(meshVertexCorrespondsTo[i], r, g, b); });
std::string segmentedMeshFile7 = outputDirectory + "/marchingBig.off";
WriteOff(segmentedMeshFile7.c_str(), verwe , leerIndices, [&](int i, int& r, int& g, int& b) { r = 175; g = 0; b = 0; });
std::string segmentedMeshFile8 = outputDirectory + "/new.off";
WriteOff(segmentedMeshFile8.c_str(), newVertices, newTriangles, [&](int i, int& r, int& g, int& b) { r = 200; g = 200; b = 0; });
//system("PAUSE");
}
int VizGeorefSpline2D::solve(void)
{
int r, c;
int p;
// No points at all
if ( _nof_points < 1 )
{
type = VIZ_GEOREF_SPLINE_ZERO_POINTS;
return(0);
}
// Only one point
if ( _nof_points == 1 )
{
type = VIZ_GEOREF_SPLINE_ONE_POINT;
return(1);
}
// Just 2 points - it is necessarily 1D case
if ( _nof_points == 2 )
{
_dx = x[1] - x[0];
_dy = y[1] - y[0];
double fact = 1.0 / ( _dx * _dx + _dy * _dy );
_dx *= fact;
_dy *= fact;
type = VIZ_GEOREF_SPLINE_TWO_POINTS;
return(2);
}
// More than 2 points - first we have to check if it is 1D or 2D case
double xmax = x[0], xmin = x[0], ymax = y[0], ymin = y[0];
double delx, dely;
double xx, yy;
double sumx = 0.0f, sumy= 0.0f, sumx2 = 0.0f, sumy2 = 0.0f, sumxy = 0.0f;
double SSxx, SSyy, SSxy;
for ( p = 0; p < _nof_points; p++ )
{
xx = x[p];
yy = y[p];
xmax = MAX( xmax, xx );
xmin = MIN( xmin, xx );
ymax = MAX( ymax, yy );
ymin = MIN( ymin, yy );
sumx += xx;
sumx2 += xx * xx;
sumy += yy;
sumy2 += yy * yy;
sumxy += xx * yy;
}
delx = xmax - xmin;
dely = ymax - ymin;
SSxx = sumx2 - sumx * sumx / _nof_points;
SSyy = sumy2 - sumy * sumy / _nof_points;
SSxy = sumxy - sumx * sumy / _nof_points;
if ( delx < 0.001 * dely || dely < 0.001 * delx ||
fabs ( SSxy * SSxy / ( SSxx * SSyy ) ) > 0.99 )
{
int p1;
type = VIZ_GEOREF_SPLINE_ONE_DIMENSIONAL;
_dx = _nof_points * sumx2 - sumx * sumx;
_dy = _nof_points * sumy2 - sumy * sumy;
double fact = 1.0 / sqrt( _dx * _dx + _dy * _dy );
_dx *= fact;
_dy *= fact;
for ( p = 0; p < _nof_points; p++ )
{
double dxp = x[p] - x[0];
double dyp = y[p] - y[0];
u[p] = _dx * dxp + _dy * dyp;
unused[p] = 1;
}
for ( p = 0; p < _nof_points; p++ )
{
int min_index = -1;
double min_u = 0;
for ( p1 = 0; p1 < _nof_points; p1++ )
{
if ( unused[p1] )
{
if ( min_index < 0 || u[p1] < min_u )
{
min_index = p1;
min_u = u[p1];
}
}
}
index[p] = min_index;
unused[min_index] = 0;
}
return(3);
}
type = VIZ_GEOREF_SPLINE_FULL;
// Make the necessary memory allocations
_nof_eqs = _nof_points + 3;
if( _nof_eqs > INT_MAX / _nof_eqs )
{
CPLError(CE_Failure, CPLE_AppDefined, "Too many coefficients. Computation aborted.");
return 0;
}
double* _AA = ( double * )VSICalloc( _nof_eqs * _nof_eqs, sizeof( double ) );
double* _Ainv = ( double * )VSICalloc( _nof_eqs * _nof_eqs, sizeof( double ) );
if( _AA == NULL || _Ainv == NULL )
{
CPLError(CE_Failure, CPLE_AppDefined, "Out-of-memory while allocating temporary arrays. Computation aborted.");
VSIFree(_AA);
VSIFree(_Ainv);
return 0;
}
// Calc the values of the matrix A
for ( r = 0; r < 3; r++ )
for ( c = 0; c < 3; c++ )
A(r,c) = 0.0;
for ( c = 0; c < _nof_points; c++ )
{
A(0,c+3) = 1.0;
A(1,c+3) = x[c];
A(2,c+3) = y[c];
A(c+3,0) = 1.0;
A(c+3,1) = x[c];
A(c+3,2) = y[c];
}
for ( r = 0; r < _nof_points; r++ )
for ( c = r; c < _nof_points; c++ )
{
A(r+3,c+3) = VizGeorefSpline2DBase_func( x[r], y[r], x[c], y[c] );
if ( r != c )
A(c+3,r+3 ) = A(r+3,c+3);
}
#if VIZ_GEOREF_SPLINE_DEBUG
for ( r = 0; r < _nof_eqs; r++ )
{
for ( c = 0; c < _nof_eqs; c++ )
fprintf(stderr, "%f", A(r,c));
fprintf(stderr, "\n");
}
#endif
int ret = 4;
#ifdef HAVE_ARMADILLO
try
{
arma::mat matA(_AA,_nof_eqs,_nof_eqs,false);
arma::mat matRHS(_nof_eqs, _nof_vars);
int row, col;
for(row = 0; row < _nof_eqs; row++)
for(col = 0; col < _nof_vars; col++)
matRHS.at(row, col) = rhs[col][row];
arma::mat matCoefs(_nof_vars, _nof_eqs);
if( !arma::solve(matCoefs, matA, matRHS) )
{
CPLError(CE_Failure, CPLE_AppDefined, "There is a problem to invert the interpolation matrix.");
ret = 0;
}
else
{
for(row = 0; row < _nof_eqs; row++)
for(col = 0; col < _nof_vars; col++)
coef[col][row] = matCoefs.at(row, col);
}
}
catch(...)
{
CPLError(CE_Failure, CPLE_AppDefined, "There is a problem to invert the interpolation matrix.");
ret = 0;
}
#else
// Invert the matrix
int status = matrixInvert( _nof_eqs, _AA, _Ainv );
if ( !status )
{
CPLError(CE_Failure, CPLE_AppDefined, "There is a problem to invert the interpolation matrix.");
ret = 0;
}
else
{
// calc the coefs
for ( int v = 0; v < _nof_vars; v++ )
for ( r = 0; r < _nof_eqs; r++ )
{
coef[v][r] = 0.0;
for ( c = 0; c < _nof_eqs; c++ )
coef[v][r] += Ainv(r,c) * rhs[v][c];
}
}
#endif
VSIFree(_AA);
VSIFree(_Ainv);
return(ret);
}
/*!
* does the anisotropic filtering
*/
int
NLD::
execute()
{
cout << "Filter options:" << endl;
cout << "tau = " << tau << endl;
cout << "time_steps = " << time_steps << endl;
cout << "precision = " << epsilon << endl;
cout << "multigrid levels = " << levels << endl;
cout << "fixed_coeffs = " << fixed_coeffs << endl;
cout << "anicoeff1 = " << anicoeff1 << endl;
cout << "anicoeff2 = " << anicoeff2 << endl;
cout << "anicoeff3 = " << anicoeff3 << endl;
cout << "dependence_type = " << dependence_type << endl;
cout << "lambda = " << lambda << endl;
cout << "integration_size_x = " << integration_size_x << endl;
cout << "integration_size_y = " << integration_size_y << endl;
cout << "integration_size_z = " << integration_size_z << endl;
cout << "geometry_type = " << gt << endl;
cout << "ip_flag = " << ip_flag << endl;
// initialise matrix
Array<int> sizes(3);
sizes.SetElement(1,dc->GetSize()[1]);
sizes.SetElement(2,dc->GetSize()[2]);
sizes.SetElement(3,dc->GetSize()[3]);
int code; // errorcode;
time_t time1, time2, timediff;
for ( int i = 1; i <= time_steps; i++ ){
cout << "Time step " << i << " :" << endl;
SparseMatrix<NeuraDataType> matA(sizes);
//delete marker1;
// initialise discretation
FV_3d27 disc(&matA,ip_flag);
disc.SetLambda(lambda);
disc.SetAniCoefficients(anicoeff1, anicoeff2, anicoeff3);
disc.SetFixedCoefficients(fixed_coeffs);
disc.SetDependenceType(dependence_type);
disc.SetIntegrationSizeX(integration_size_x);
disc.SetIntegrationSizeY(integration_size_y);
disc.SetIntegrationSizeZ(integration_size_z);
disc.SetGeometryType(gt);
// initialise right hand side
MultiVector<NeuraDataType> rhs;
dc->CopyIntoMultiVector(rhs);
//delete marker2;
// initialise solution
MultiVector<NeuraDataType> sol(rhs);
sol.SetZero();
// initialise multigrid
Multigrid<NeuraDataType> multigrid;
// important: precision has to be set before initializing multigrid!
multigrid.SetPrecision(epsilon);
// set multigrid properties
// make sure that precision was setted before!
multigrid.Initialise(REGULAR, MATRIX_DEPENDENT_INTERPOLATION,
MATRIX_DEPENDENT_RESTRICTION, alpha,
LEXICOGRAPHIC_GAUSS_SEIDEL, 2, 2, V, BICGSTAB, NODES, levels);
multigrid.SaveProlongation();
multigrid.SaveRestriction();
multigrid.SetMaximumSteps(50);
dc->CopyIntoMultiVector(rhs);
dc->CopyIntoMultiVector(sol);
cout << "discretize..." << endl;
time1 = time(NULL);
code = disc.discretize(*dc);
if ( code ) return code;
cout << "NLD: Ich komme hier an!" << endl;
disc.SetTimeDependent(tau);
time2 = time(NULL);
cout << "...discretation finished" << endl;
timediff = time2-time1;
cout << "discretation took " << timediff << " seconds" << endl;
cout << "BuildMatrixHierarchy..." << endl;
if ( levels > 1 )
{
if (
( !pow_of_two(dc->GetSize()[1]-1) ) ||
( !pow_of_two(dc->GetSize()[2]-1) ) ||
( !pow_of_two(dc->GetSize()[3]-1) )
)
{
return SIZE_NOT_POW_OF_TWO_PLUS_ONE;
}
}
time1 = time(NULL);
multigrid.BuildMatrixHierarchy(matA);
time2 = time(NULL);
cout << "...done" << endl;
timediff = time2-time1;
cout << "building matrix hierarchy took " << timediff << " seconds" << endl;
cout << "solve by multigrid method..." << endl;
time1 = time(NULL);
multigrid.Solve(rhs,sol);
time2 = time(NULL);
cout << "......done" << endl;
timediff = time2-time1;
cout << "soluting took " << timediff << " seconds" << endl;
dc->CopyFromMultiVector(sol);
}
cout <<"quit nld now" << endl;
return OK;
}
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;
}
