Mapper::DP* Mapper::updateMap(DP* newMap, const PM::TransformationParameters Ticp, bool updateExisting) { timer t; // Prepare empty field if not existing // FIXME: this is only needed for the none overlaping part if(newMap->descriptorExists("probabilityStatic") == false) { //newMap->addDescriptor("probabilityStatic", PM::Matrix::Zero(1, newMap->features.cols())); newMap->addDescriptor("probabilityStatic", PM::Matrix::Constant(1, newMap->features.cols(), priorStatic)); } if(newMap->descriptorExists("probabilityDynamic") == false) { //newMap->addDescriptor("probabilityDynamic", PM::Matrix::Zero(1, newMap->features.cols())); newMap->addDescriptor("probabilityDynamic", PM::Matrix::Constant(1, newMap->features.cols(), priorDyn)); } if(newMap->descriptorExists("debug") == false) { newMap->addDescriptor("debug", PM::Matrix::Zero(1, newMap->features.cols())); } if (!updateExisting) { // FIXME: correct that, ugly cout << "Jumping map creation" << endl; *newMap = transformation->compute(*newMap, Ticp); mapPostFilters.apply(*newMap); return newMap; } // FIXME: only for debug mapPointCloud->getDescriptorViewByName("debug") = PM::Matrix::Zero(1, mapPointCloud->features.cols()); const int newMapPts(newMap->features.cols()); const int mapPtsCount(mapPointCloud->features.cols()); // Build a range image of the reading point cloud (local coordinates) PM::Matrix radius_newMap = newMap->features.topRows(3).colwise().norm(); PM::Matrix angles_newMap(2, newMapPts); // 0=inclination, 1=azimuth // No atan in Eigen, so we are for to loop through it... for(int i=0; i<newMapPts; i++) { const float ratio = newMap->features(2,i)/radius_newMap(0,i); angles_newMap(0,i) = acos(ratio); angles_newMap(1,i) = atan2(newMap->features(1,i), newMap->features(0,i)); } std::shared_ptr<NNS> kdtree; kdtree.reset( NNS::create(angles_newMap)); //-------------- Global map ------------------------------ // Transform the global map in local coordinates DP mapLocal = transformation->compute(*mapPointCloud, Ticp.inverse()); // ROI: Region of Interest // We reduce the global map to the minimum for the processing //const float sensorMaxRange = 80.0; // ICRA PM::Matrix globalId(1, mapPtsCount); int ROIpts = 0; int notROIpts = 0; // Split mapLocal DP mapLocalROI(mapLocal.createSimilarEmpty()); for (int i = 0; i < mapPtsCount; i++) { // Copy the points of the ROI in a new map if (mapLocal.features.col(i).head(3).norm() < sensorMaxRange) { mapLocalROI.setColFrom(ROIpts, mapLocal, i); globalId(0,ROIpts) = i; ROIpts++; } else // Remove the points of the ROI from the global map { mapLocal.setColFrom(notROIpts, mapLocal, i); notROIpts++; } } mapLocalROI.conservativeResize(ROIpts); mapLocal.conservativeResize(notROIpts); // Convert the map in spherical coordinates PM::Matrix radius_map = mapLocalROI.features.topRows(3).colwise().norm(); PM::Matrix angles_map(2, ROIpts); // 0=inclination, 1=azimuth // No atan in Eigen, so we are looping through it... // TODO: check for: A.binaryExpr(B, std::ptr_fun(atan2)) for(int i=0; i < ROIpts; i++) { const float ratio = mapLocalROI.features(2,i)/radius_map(0,i); //if(ratio < -1 || ratio > 1) //cout << "Error angle!" << endl; angles_map(0,i) = acos(ratio); angles_map(1,i) = atan2(mapLocalROI.features(1,i), mapLocalROI.features(0,i)); } // Prepare access to descriptors DP::View viewOn_Msec_overlap = newMap->getDescriptorViewByName("stamps_Msec"); DP::View viewOn_sec_overlap = newMap->getDescriptorViewByName("stamps_sec"); DP::View viewOn_nsec_overlap = newMap->getDescriptorViewByName("stamps_nsec"); DP::View viewOnProbabilityStatic = mapLocalROI.getDescriptorViewByName("probabilityStatic"); DP::View viewOnProbabilityDynamic = mapLocalROI.getDescriptorViewByName("probabilityDynamic"); DP::View viewDebug = mapLocalROI.getDescriptorViewByName("debug"); DP::View viewOn_normals_map = mapLocalROI.getDescriptorViewByName("normals"); DP::View viewOn_Msec_map = mapLocalROI.getDescriptorViewByName("stamps_Msec"); DP::View viewOn_sec_map = mapLocalROI.getDescriptorViewByName("stamps_sec"); DP::View viewOn_nsec_map = mapLocalROI.getDescriptorViewByName("stamps_nsec"); // Search for the nearest point in newMap Matches::Dists dists(1, ROIpts); Matches::Ids ids(1, ROIpts); kdtree->knn(angles_map, ids, dists, 1, 0, NNS::ALLOW_SELF_MATCH, maxAngle); // update probability of being dynamic for all points in ROI for(int i=0; i < ROIpts; i++) { const int mapId = i; viewDebug(0,mapId) = 1; //FIXME: debug if(dists(i) != numeric_limits<float>::infinity()) { const int readId = ids(0,i); //const int mapId = globalId(0,i); // in local coordinates const Eigen::Vector3f readPt = newMap->features.col(readId).head(3); const Eigen::Vector3f mapPt = mapLocalROI.features.col(i).head(3); const Eigen::Vector3f mapPt_n = mapPt.normalized(); const float delta = (readPt - mapPt).norm(); const float d_max = eps_a * readPt.norm(); const Eigen::Vector3f normal_map = viewOn_normals_map.col(mapId); // Weight for dynamic elements const float w_v = eps + (1 - eps)*fabs(normal_map.dot(mapPt_n)); const float w_d1 = eps + (1 - eps)*(1 - sqrt(dists(i))/maxAngle); const float offset = delta - eps_d; float w_d2 = 1; if(delta < eps_d || mapPt.norm() > readPt.norm()) { w_d2 = eps; } else { if (offset < d_max) { w_d2 = eps + (1 - eps )*offset/d_max; } } float w_p2 = eps; if(delta < eps_d) { w_p2 = 1; } else { if(offset < d_max) { w_p2 = eps + (1 - eps)*(1 - offset/d_max); } } // We don't update point behind the reading if((readPt.norm() + eps_d + d_max) >= mapPt.norm()) { const float lastDyn = viewOnProbabilityDynamic(0,mapId); const float lastStatic = viewOnProbabilityStatic(0, mapId); const float c1 = (1 - (w_v*(1 - w_d1))); const float c2 = w_v*(1 - w_d1); //Lock dynamic point to stay dynamic under a threshold if(lastDyn < maxDyn) { viewOnProbabilityDynamic(0,mapId) = c1*lastDyn + c2*w_d2*((1 - alpha)*lastStatic + beta*lastDyn); viewOnProbabilityStatic(0, mapId) = c1*lastStatic + c2*w_p2*(alpha*lastStatic + (1 - beta)*lastDyn); } else { viewOnProbabilityStatic(0,mapId) = eps; viewOnProbabilityDynamic(0,mapId) = 1-eps; } // normalization const float sumZ = viewOnProbabilityDynamic(0,mapId) + viewOnProbabilityStatic(0, mapId); assert(sumZ >= eps); viewOnProbabilityDynamic(0,mapId) /= sumZ; viewOnProbabilityStatic(0,mapId) /= sumZ; //viewDebug(0,mapId) =viewOnProbabilityDynamic(0, mapId); //viewDebug(0,mapId) = w_d2; // Refresh time viewOn_Msec_map(0,mapId) = viewOn_Msec_overlap(0,readId); viewOn_sec_map(0,mapId) = viewOn_sec_overlap(0,readId); viewOn_nsec_map(0,mapId) = viewOn_nsec_overlap(0,readId); } } } // Compute density const int mapLocalROIPts = mapLocalROI.features.cols(); cout << "build first kdtree with " << mapLocalROIPts << endl; // build and populate NNS std::shared_ptr<NNS> kdtree2; kdtree2.reset( NNS::create(mapLocalROI.features, mapLocalROI.features.rows() - 1, NNS::KDTREE_LINEAR_HEAP, NNS::TOUCH_STATISTICS)); PM::Matches matches_overlap( Matches::Dists(1, newMapPts), Matches::Ids(1, newMapPts) ); kdtree2->knn(newMap->features, matches_overlap.ids, matches_overlap.dists, 1, 0, NNS::ALLOW_SELF_MATCH, maxDistNewPoint); //-------------- New point cloud ------------------------------ DP newMapOverlap(newMap->createSimilarEmpty());// Not used for now PM::Matrix minDist = PM::Matrix::Constant(1,mapLocalROIPts, std::numeric_limits<float>::max()); // Reduce newMap to its none overlapping part int ptsOut = 0; int ptsIn = 0; bool hasSensorNoise = mapLocalROI.descriptorExists("simpleSensorNoise") && newMap->descriptorExists("simpleSensorNoise"); if(hasSensorNoise) // Split and update point with lower noise { DP::View viewOn_noise_mapLocal = mapLocalROI.getDescriptorViewByName("simpleSensorNoise"); DP::View viewOn_noise_newMap = newMap->getDescriptorViewByName("simpleSensorNoise"); for (int i = 0; i < newMapPts; ++i) { const int localMapId = matches_overlap.ids(i); if (matches_overlap.dists(i) == numeric_limits<float>::infinity()) { newMap->setColFrom(ptsOut, *newMap, i); ptsOut++; } else // Overlapping points { // Update point with lower sensor noise if(viewOn_noise_newMap(0,i) < viewOn_noise_mapLocal(0,localMapId)) { if(matches_overlap.dists(i) < minDist(localMapId)) { minDist(localMapId) = matches_overlap.dists(i); const float debug = viewDebug(0, localMapId); const float probDyn = viewOnProbabilityDynamic(0, localMapId); const float probStatic = viewOnProbabilityStatic(0, localMapId); mapLocalROI.setColFrom(localMapId, *newMap, i); viewDebug(0, localMapId) = 2; viewOnProbabilityDynamic(0, localMapId) = probDyn; viewOnProbabilityStatic(0, localMapId) = probStatic; } } newMapOverlap.setColFrom(ptsIn, *newMap, i); ptsIn++; } } } else // Only split newMap { for (int i = 0; i < newMapPts; ++i) { if (matches_overlap.dists(i) == numeric_limits<float>::infinity()) { newMap->setColFrom(ptsOut, *newMap, i); ptsOut++; } else // Overlapping points { newMapOverlap.setColFrom(ptsIn, *newMap, i); ptsIn++; } } } // shrink the newMap to the new information newMap->conservativeResize(ptsOut); newMapOverlap.conservativeResize(ptsIn); cout << "ptsOut=" << ptsOut << ", ptsIn=" << ptsIn << endl; // Publish debug //if (debugPub.getNumSubscribers()) //{ // debugPub.publish(PointMatcher_ros::pointMatcherCloudToRosMsg<float>(*newMap, mapFrame, mapCreationTime)); //} //no_overlap.addDescriptor("debug", PM::Matrix::Zero(1, no_overlap.features.cols())); // Add the ROI to the none overlapping part newMap->concatenate(mapLocalROI); // Apply the filters only on the ROI mapPostFilters.apply(*newMap); // Add the rest of the map newMap->concatenate(mapLocal); // Transform the map back to global coordinates *newMap = transformation->compute(*newMap, Ticp); cout << "... end map creation" << endl; ROS_INFO_STREAM("[TIME] New map available (" << newMap->features.cols() << " pts), update took " << t.elapsed() << " [s]"); return newMap; }
bool Mapper::getBoundedMap(ethzasl_icp_mapper::GetBoundedMap::Request &req, ethzasl_icp_mapper::GetBoundedMap::Response &res) { if (!mapPointCloud) return false; const float max_x = req.topRightCorner.x; const float max_y = req.topRightCorner.y; const float max_z = req.topRightCorner.z; const float min_x = req.bottomLeftCorner.x; const float min_y = req.bottomLeftCorner.y; const float min_z = req.bottomLeftCorner.z; cerr << "min [" << min_x << ", " << min_y << ", " << min_z << "] " << endl; cerr << "max [" << max_x << ", " << max_y << ", " << max_z << "] " << endl; tf::StampedTransform stampedTr; Eigen::Affine3d eigenTr; tf::poseMsgToEigen(req.mapCenter, eigenTr); Eigen::MatrixXf T = eigenTr.matrix().inverse().cast<float>(); //const Eigen::MatrixXf T = eigenTr.matrix().cast<float>(); cerr << "T:" << endl << T << endl; T = transformation->correctParameters(T); // FIXME: do we need a mutex here? const DP centeredPointCloud = transformation->compute(*mapPointCloud, T); DP cutPointCloud = centeredPointCloud.createSimilarEmpty(); cerr << centeredPointCloud.features.topLeftCorner(3, 10) << endl; cerr << T << endl; int newPtCount = 0; for(int i=0; i < centeredPointCloud.features.cols(); i++) { const float x = centeredPointCloud.features(0,i); const float y = centeredPointCloud.features(1,i); const float z = centeredPointCloud.features(2,i); if(x < max_x && x > min_x && y < max_y && y > min_y && z < max_z && z > min_z ) { cutPointCloud.setColFrom(newPtCount, centeredPointCloud, i); newPtCount++; } } cerr << "Extract " << newPtCount << " points from the map" << endl; cutPointCloud.conservativeResize(newPtCount); cutPointCloud = transformation->compute(cutPointCloud, T.inverse()); // Send the resulting point cloud in ROS format res.boundedMap = PointMatcher_ros::pointMatcherCloudToRosMsg<float>(cutPointCloud, mapFrame, ros::Time::now()); return true; }