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; }