void ICPOdometry::initICPModel(const DeviceArray2D<unsigned short>& depth, const float depthCutoff, const Eigen::Matrix4f & modelPose) { depth_tmp[0] = depth; for(int i = 1; i < NUM_PYRS; ++i) { pyrDown(depth_tmp[i - 1], depth_tmp[i]); } for(int i = 0; i < NUM_PYRS; ++i) { createVMap(intr(i), depth_tmp[i], vmaps_g_prev_[i], depthCutoff); createNMap(vmaps_g_prev_[i], nmaps_g_prev_[i]); } cudaDeviceSynchronize(); Eigen::Matrix<float, 3, 3, Eigen::RowMajor> Rcam = modelPose.topLeftCorner(3, 3); Eigen::Vector3f tcam = modelPose.topRightCorner(3, 1); Mat33 & device_Rcam = device_cast<Mat33>(Rcam); float3& device_tcam = device_cast<float3>(tcam); if (modelPose != Eigen::Matrix4f::Identity()) for(int i = 0; i < NUM_PYRS; ++i) tranformMaps(vmaps_g_prev_[i], nmaps_g_prev_[i], device_Rcam, device_tcam, vmaps_g_prev_[i], nmaps_g_prev_[i]); cudaDeviceSynchronize(); }
void ICPSlowdometry::initICPModel(unsigned short * depth, const float depthCutoff, const Eigen::Matrix4f & modelPose) { depth_tmp[0].upload(depth, sizeof(unsigned short) * width, height, width); for(int i = 1; i < NUM_PYRS; ++i) { pyrDown(depth_tmp[i - 1], depth_tmp[i]); } for(int i = 0; i < NUM_PYRS; ++i) { createVMap(intr(i), depth_tmp[i], vmaps_g_prev_[i], depthCutoff); createNMap(vmaps_g_prev_[i], nmaps_g_prev_[i]); } cudaDeviceSynchronize(); Eigen::Matrix<float, 3, 3, Eigen::RowMajor> Rcam = modelPose.topLeftCorner(3, 3); Eigen::Vector3f tcam = modelPose.topRightCorner(3, 1); Mat33 & device_Rcam = device_cast<Mat33>(Rcam); float3& device_tcam = device_cast<float3>(tcam); for(int i = 0; i < NUM_PYRS; ++i) { tranformMaps(vmaps_g_prev_[i], nmaps_g_prev_[i], device_Rcam, device_tcam, vmaps_g_prev_[i], nmaps_g_prev_[i]); } cudaDeviceSynchronize(); }
void DiriniVisualizer::addCloud(pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud_, Eigen::Matrix3f R, Eigen::Vector3f t) { Eigen::Matrix4f H = Eigen::Matrix4f::Identity(); H.topLeftCorner(3,3) = R; H.topRightCorner(3,1) = t; addCloud(cloud_, H); }
void DiriniVisualizer::addCloud(boost::shared_ptr<std::vector<Eigen::Vector3i> > points, Eigen::Matrix3f R, Eigen::Vector3f t) { Eigen::Matrix4f H = Eigen::Matrix4f::Identity(); H.topLeftCorner(3,3) = R; H.topRightCorner(3,1) = t; addCloud(points, H); }
int main() { Eigen::Matrix4f m; m << 1, 2, 3, 4, 5, 6, 7, 8, 9, 10,11,12, 13,14,15,16; cout << "m.leftCols(2) =" << endl << m.leftCols(2) << endl << endl; cout << "m.bottomRows<2>() =" << endl << m.bottomRows<2>() << endl << endl; m.topLeftCorner(1,3) = m.bottomRightCorner(3,1).transpose(); cout << "After assignment, m = " << endl << m << endl; }
void OBJCTXT<_DOF6>::findCorrespondences3(const OBJCTXT &ctxt, std::vector<SCOR> &cors, const DOF6 &tf) { map_cors_.clear(); Eigen::Matrix4f M = tf.getTF4().inverse(); Eigen::Vector3f t=M.col(3).head<3>(); Eigen::Matrix3f R=M.topLeftCorner(3,3); const float thr = tf.getRotationVariance()+0.05f; for(size_t j=0; j<ctxt.objs_.size(); j++) { OBJECT obj = *ctxt.objs_[j]; obj.transform(R,t,0,0); for(size_t i=0; i<objs_.size(); i++) if( (obj.getData().getBB().preassumption(objs_[i]->getData().getBB())>=std::cos(thr+objs_[i]->getData().getBB().ratio())) && obj.intersectsBB(*objs_[i], tf.getRotationVariance(),tf.getTranslationVariance()) && obj.getData().extensionMatch(objs_[i]->getData(),0.7f,0) ) { if(obj.intersectsPts(*objs_[i], tf.getRotationVariance(),tf.getTranslationVariance()) || objs_[i]->intersectsPts(obj, tf.getRotationVariance(),tf.getTranslationVariance())) { //seccond check map_cors_[ctxt.objs_[j]].push_back(cors.size()); SCOR c; c.a = objs_[i]; c.b = ctxt.objs_[j]; cors.push_back(c); } } } #ifdef DEBUG_ ROS_INFO("found %d correspondences (%d %d)", (int)cors.size(), (int)objs_.size(), (int)ctxt.objs_.size()); for(typename std::vector<SCOR>::iterator it = cors.begin(); it!=cors.end(); it++) { Debug::Interface::get().addArrow(it->a->getNearestPoint(),it->b->getNearestPoint(), 0,255,0); } #endif }
void RGBDOdometry::initICPModel(GPUTexture * predictedVertices, GPUTexture * predictedNormals, const float depthCutoff, const Eigen::Matrix4f & modelPose) { cudaArray * textPtr; cudaGraphicsMapResources(1, &predictedVertices->cudaRes); cudaGraphicsSubResourceGetMappedArray(&textPtr, predictedVertices->cudaRes, 0, 0); cudaMemcpyFromArray(vmaps_tmp.ptr(), textPtr, 0, 0, vmaps_tmp.sizeBytes(), cudaMemcpyDeviceToDevice); cudaGraphicsUnmapResources(1, &predictedVertices->cudaRes); cudaGraphicsMapResources(1, &predictedNormals->cudaRes); cudaGraphicsSubResourceGetMappedArray(&textPtr, predictedNormals->cudaRes, 0, 0); cudaMemcpyFromArray(nmaps_tmp.ptr(), textPtr, 0, 0, nmaps_tmp.sizeBytes(), cudaMemcpyDeviceToDevice); cudaGraphicsUnmapResources(1, &predictedNormals->cudaRes); copyMaps(vmaps_tmp, nmaps_tmp, vmaps_g_prev_[0], nmaps_g_prev_[0]); for(int i = 1; i < NUM_PYRS; ++i) { resizeVMap(vmaps_g_prev_[i - 1], vmaps_g_prev_[i]); resizeNMap(nmaps_g_prev_[i - 1], nmaps_g_prev_[i]); } Eigen::Matrix<float, 3, 3, Eigen::RowMajor> Rcam = modelPose.topLeftCorner(3, 3); Eigen::Vector3f tcam = modelPose.topRightCorner(3, 1); mat33 device_Rcam = Rcam; float3 device_tcam = *reinterpret_cast<float3*>(tcam.data()); for(int i = 0; i < NUM_PYRS; ++i) { tranformMaps(vmaps_g_prev_[i], nmaps_g_prev_[i], device_Rcam, device_tcam, vmaps_g_prev_[i], nmaps_g_prev_[i]); } cudaDeviceSynchronize(); }
template <typename PointSource, typename PointTarget> inline void pcl::GeneralizedIterativeClosestPoint<PointSource, PointTarget>::computeTransformation (PointCloudSource &output, const Eigen::Matrix4f& guess) { pcl::IterativeClosestPoint<PointSource, PointTarget>::initComputeReciprocal (); using namespace std; // Difference between consecutive transforms double delta = 0; // Get the size of the target const size_t N = indices_->size (); // Set the mahalanobis matrices to identity mahalanobis_.resize (N, Eigen::Matrix3d::Identity ()); // Compute target cloud covariance matrices if (target_covariances_.empty ()) computeCovariances<PointTarget> (target_, tree_, target_covariances_); // Compute input cloud covariance matrices if (input_covariances_.empty ()) computeCovariances<PointSource> (input_, tree_reciprocal_, input_covariances_); base_transformation_ = guess; nr_iterations_ = 0; converged_ = false; double dist_threshold = corr_dist_threshold_ * corr_dist_threshold_; std::vector<int> nn_indices (1); std::vector<float> nn_dists (1); while(!converged_) { size_t cnt = 0; std::vector<int> source_indices (indices_->size ()); std::vector<int> target_indices (indices_->size ()); // guess corresponds to base_t and transformation_ to t Eigen::Matrix4d transform_R = Eigen::Matrix4d::Zero (); for(size_t i = 0; i < 4; i++) for(size_t j = 0; j < 4; j++) for(size_t k = 0; k < 4; k++) transform_R(i,j)+= double(transformation_(i,k)) * double(guess(k,j)); Eigen::Matrix3d R = transform_R.topLeftCorner<3,3> (); for (size_t i = 0; i < N; i++) { PointSource query = output[i]; query.getVector4fMap () = guess * query.getVector4fMap (); query.getVector4fMap () = transformation_ * query.getVector4fMap (); if (!searchForNeighbors (query, nn_indices, nn_dists)) { PCL_ERROR ("[pcl::%s::computeTransformation] Unable to find a nearest neighbor in the target dataset for point %d in the source!\n", getClassName ().c_str (), (*indices_)[i]); return; } // Check if the distance to the nearest neighbor is smaller than the user imposed threshold if (nn_dists[0] < dist_threshold) { Eigen::Matrix3d &C1 = input_covariances_[i]; Eigen::Matrix3d &C2 = target_covariances_[nn_indices[0]]; Eigen::Matrix3d &M = mahalanobis_[i]; // M = R*C1 M = R * C1; // temp = M*R' + C2 = R*C1*R' + C2 Eigen::Matrix3d temp = M * R.transpose(); temp+= C2; // M = temp^-1 M = temp.inverse (); source_indices[cnt] = static_cast<int> (i); target_indices[cnt] = nn_indices[0]; cnt++; } } // Resize to the actual number of valid correspondences source_indices.resize(cnt); target_indices.resize(cnt); /* optimize transformation using the current assignment and Mahalanobis metrics*/ previous_transformation_ = transformation_; //optimization right here try { rigid_transformation_estimation_(output, source_indices, *target_, target_indices, transformation_); /* compute the delta from this iteration */ delta = 0.; for(int k = 0; k < 4; k++) { for(int l = 0; l < 4; l++) { double ratio = 1; if(k < 3 && l < 3) // rotation part of the transform ratio = 1./rotation_epsilon_; else ratio = 1./transformation_epsilon_; double c_delta = ratio*fabs(previous_transformation_(k,l) - transformation_(k,l)); if(c_delta > delta) delta = c_delta; } } } catch (PCLException &e) { PCL_DEBUG ("[pcl::%s::computeTransformation] Optimization issue %s\n", getClassName ().c_str (), e.what ()); break; } nr_iterations_++; // Check for convergence if (nr_iterations_ >= max_iterations_ || delta < 1) { converged_ = true; previous_transformation_ = transformation_; PCL_DEBUG ("[pcl::%s::computeTransformation] Convergence reached. Number of iterations: %d out of %d. Transformation difference: %f\n", getClassName ().c_str (), nr_iterations_, max_iterations_, (transformation_ - previous_transformation_).array ().abs ().sum ()); } else PCL_DEBUG ("[pcl::%s::computeTransformation] Convergence failed\n", getClassName ().c_str ()); } //for some reason the static equivalent methode raises an error // final_transformation_.block<3,3> (0,0) = (transformation_.block<3,3> (0,0)) * (guess.block<3,3> (0,0)); // final_transformation_.block <3, 1> (0, 3) = transformation_.block <3, 1> (0, 3) + guess.rightCols<1>.block <3, 1> (0, 3); final_transformation_.topLeftCorner (3,3) = previous_transformation_.topLeftCorner (3,3) * guess.topLeftCorner (3,3); final_transformation_(0,3) = previous_transformation_(0,3) + guess(0,3); final_transformation_(1,3) = previous_transformation_(1,3) + guess(1,3); final_transformation_(2,3) = previous_transformation_(2,3) + guess(2,3); }
int main(int argc, char* argv[]) { glutInit(&argc, argv); glutInitDisplayMode(GLUT_SINGLE | GLUT_RGB | GLUT_DEPTH); glutInitWindowSize(1,1); glutCreateWindow(""); glutHideWindow(); GLenum err = glewInit(); if (err != GLEW_OK) { cerr << "Error:" << glewGetErrorString(err) << endl; return 1; } if (!parseargs(argc, argv)) return 1; PolygonMesh::Ptr mesh(new PolygonMesh()); cout << "Loading mesh geometry...." << endl; io::loadPolygonFile(argv[1], *mesh); PointCloud<PointXYZRGB>::Ptr cloud(new PointCloud<PointXYZRGB>()); { PointCloud<PointXYZ> tmp; fromPCLPointCloud2(mesh->cloud, tmp); cloud->points.resize(tmp.size()); for (size_t i = 0; i < tmp.size(); ++i) { cloud->points[i].x = tmp[i].x; cloud->points[i].y = tmp[i].y; cloud->points[i].z = tmp[i].z; } } PlaneOrientationFinder of(mesh, resolution/2); of.computeNormals(ccw); Mesh m(mesh,true); cout << "Done loading mesh geometry" << endl; vector<int> wallindices; vector<int> floorindices; ColorHelper loader(image_flip_x, image_flip_y); WallFinder wf(&of, resolution); if (do_wallfinding) { cout << "===== WALLFINDING =====" << endl; if (wallinput) { cout << "Loading wall files..." << endl; wf.loadWalls(wallfile, m.types); cout << "Done loading wall files..." << endl; of.normalize(); } else { cout << "Analyzing geometry..." << endl; if (!of.computeOrientation()) { cout << "Error computing orientation! Non-triangle mesh!" << endl; } cout << "Done analyzing geometry" << endl; of.normalize(); cout << "Finding walls..." << endl; wf.findFloorAndCeiling(m.types, anglethreshold); wf.findWalls(m.types, wallthreshold, minlength, anglethreshold); cout << "Done finding walls" << endl; } if (flipfloorceiling) { for (int i = 0; i < m.types.size(); ++i) { if (m.types[i] == WallFinder::LABEL_CEILING) m.types[i] = WallFinder::LABEL_FLOOR; else if (m.types[i] == WallFinder::LABEL_FLOOR) m.types[i] = WallFinder::LABEL_CEILING; } } for (int i = 0; i < m.types.size(); ++i) { if (m.types[i] == WallFinder::LABEL_WALL) wallindices.push_back(i); else if (m.types[i] == WallFinder::LABEL_FLOOR) floorindices.push_back(i); } if (output_wall) wf.saveWalls(walloutfile, m.types); cout << "=======================" << endl; } int numlights = 0; if (do_reprojection) { cout << "===== REPROJECTING =====" << endl; if (input) { cout << "Loading reprojection input files..." << endl; loader.readCameraFile(camfile); cout << "Reading input files..." << endl; loader.load(camfile, ColorHelper::READ_COLOR); if (use_confidence_files) { loader.load(camfile, ColorHelper::READ_CONFIDENCE); } cout << "Done reading " << loader.size() << " color images" << endl; if (do_wallfinding) { vector<char> tmp; swap(tmp, m.types); numlights = m.readSamples(infile); swap (m.types, tmp); } else { numlights = m.readSamples(infile); } cout << "Done loading reprojection files" << endl; if (hdr_threshold > 0) { numlights = clusterLights(m, hdr_threshold, minlightsize); cout << "Done clustering " << numlights << " lights" << endl; } } else { if (hdr_threshold < 0) hdr_threshold = 10.0; if (all_project) { cout << "Reading input files..." << endl; loader.load(camfile, ColorHelper::READ_COLOR | ColorHelper::READ_DEPTH); if (use_confidence_files) { loader.load(camfile, ColorHelper::READ_CONFIDENCE); } cout << "Done reading " << loader.size() << " color images" << endl; cout << "Reprojecting..." << endl; reproject(loader, m, hdr_threshold); } else { loader.load(camfile, 0); loader.load(project, ColorHelper::READ_COLOR | ColorHelper::READ_DEPTH); if (use_confidence_files) { loader.load(project, ColorHelper::READ_CONFIDENCE); } cout << "Reprojecting..." << endl; reproject((const float*) loader.getImage(project), loader.getConfidenceMap(project), loader.getDepthMap(project), loader.getCamera(project), m, hdr_threshold); } cout << "Done reprojecting; clustering lights..." << endl; numlights = clusterLights(m, hdr_threshold, minlightsize); cout << "Done clustering " << numlights << " lights" << endl; } if (output_reprojection) m.writeSamples(outfile); if (coloredfile.length()) m.writeColoredMesh(coloredfile, displayscale); m.computeColorsOGL(); hemicuberesolution = max(hemicuberesolution, loader.getCamera(0)->width); hemicuberesolution = max(hemicuberesolution, loader.getCamera(0)->height); cout << "========================" << endl; } InverseRender ir(&m, numlights, hemicuberesolution); vector<SampleData> walldata, floordata; // Only do inverse rendering with full reprojection and wall labels if (do_sampling && (input || all_project) && (wallinput || do_wallfinding)) { cout << "==== SAMPLING SCENE ====" << endl; if (read_eq) { ir.loadVariablesBinary(walldata, samplefile + ".walls"); if (do_texture) { ir.loadVariablesBinary(floordata, samplefile + ".floors"); } } else { ir.computeSamples(walldata, wallindices, numsamples, discardthreshold); if (do_texture) { ir.computeSamples(floordata, floorindices, numsamples, discardthreshold); } if (write_eq) { ir.writeVariablesBinary(walldata, sampleoutfile + ".walls"); if (do_texture) { ir.writeVariablesBinary(floordata, sampleoutfile + ".floors"); } } if (write_matlab) { ir.writeVariablesMatlab(walldata, matlabsamplefile); } } cout << "========================" << endl; ir.setNumRansacIters(numRansacIters); ir.setMaxPercentErr(maxPercentErr); ir.solve(walldata); Texture tex; if (do_texture) { // Prepare floor plane Eigen::Matrix4f t = of.getNormalizationTransform().inverse(); Eigen::Vector3f floornormal(0,flipfloorceiling?-1:1,0); floornormal = t.topLeftCorner(3,3)*floornormal; Eigen::Vector4f floorpoint(0, wf.floorplane, 0, 1); floorpoint = t*floorpoint; R3Plane floorplane(eigen2gaps(floorpoint.head(3)).Point(), eigen2gaps(floornormal)); loader.load(camfile, use_confidence_files); cout << "Solving texture..." << endl; ir.solveTexture(floordata, &loader, floorplane, tex); cout << "Done solving texture..." << endl; if (tex.size > 0) { ColorHelper::writeExrImage(texfile, tex.texture, tex.size, tex.size); } } if (radfile != "") { outputRadianceFile(radfile, wf, m, ir); } if (plyfile != "") { outputPlyFile(plyfile, wf, m, ir); } if (pbrtfile != "") { outputPbrtFile(pbrtfile, wf, m, ir, tex, loader.getCamera(0), do_texture?texfile:""); } } cout << "DONE PROCESSING" << endl; if (display) { InvRenderVisualizer irv(cloud, loader, wf, ir); if (project_debug) irv.recalculateColors(InvRenderVisualizer::LABEL_REPROJECT_DEBUG); irv.visualizeCameras(camera); irv.visualizeWalls(); irv.addSamples(walldata); irv.loop(); } return 0; }
/* ---[ */ int main (int argc, char** argv) { print_info ("Transform a cloud. For more information, use: %s -h\n", argv[0]); bool help = false; parse_argument (argc, argv, "-h", help); if (argc < 3 || help) { printHelp (argc, argv); return (-1); } // Parse the command line arguments for .pcd files std::vector<int> p_file_indices; p_file_indices = parse_file_extension_argument (argc, argv, ".pcd"); if (p_file_indices.size () != 2) { print_error ("Need one input PCD file and one output PCD file to continue.\n"); return (-1); } // Initialize the transformation matrix Eigen::Matrix4f tform; tform.setIdentity (); // Command line parsing float dx, dy, dz; std::vector<float> values; if (parse_3x_arguments (argc, argv, "-trans", dx, dy, dz) > -1) { tform (0, 3) = dx; tform (1, 3) = dy; tform (2, 3) = dz; } if (parse_x_arguments (argc, argv, "-quat", values) > -1) { if (values.size () == 4) { const float& x = values[0]; const float& y = values[1]; const float& z = values[2]; const float& w = values[3]; tform.topLeftCorner (3, 3) = Eigen::Matrix3f (Eigen::Quaternionf (w, x, y, z)); } else { print_error ("Wrong number of values given (%zu): ", values.size ()); print_error ("The quaternion specified with -quat must contain 4 elements (w,x,y,z).\n"); } } if (parse_x_arguments (argc, argv, "-axisangle", values) > -1) { if (values.size () == 4) { const float& ax = values[0]; const float& ay = values[1]; const float& az = values[2]; const float& theta = values[3]; tform.topLeftCorner (3, 3) = Eigen::Matrix3f (Eigen::AngleAxisf (theta, Eigen::Vector3f (ax, ay, az))); } else { print_error ("Wrong number of values given (%zu): ", values.size ()); print_error ("The rotation specified with -axisangle must contain 4 elements (ax,ay,az,theta).\n"); } } if (parse_x_arguments (argc, argv, "-matrix", values) > -1) { if (values.size () == 9 || values.size () == 16) { int n = values.size () == 9 ? 3 : 4; for (int r = 0; r < n; ++r) for (int c = 0; c < n; ++c) tform (r, c) = values[n*r+c]; } else { print_error ("Wrong number of values given (%zu): ", values.size ()); print_error ("The transformation specified with -matrix must be 3x3 (9) or 4x4 (16).\n"); } } // Load the first file pcl::PCLPointCloud2::Ptr cloud (new pcl::PCLPointCloud2); if (!loadCloud (argv[p_file_indices[0]], *cloud)) return (-1); // Apply the transform pcl::PCLPointCloud2 output; compute (cloud, output, tform); // Check if a scaling parameter has been given double divider[3]; if (parse_3x_arguments (argc, argv, "-scale", divider[0], divider[1], divider[2]) > -1) { print_highlight ("Scaling XYZ data with the following values: %f, %f, %f\n", divider[0], divider[1], divider[2]); scaleInPlace (output, divider); } // Save into the second file saveCloud (argv[p_file_indices[1]], output); }
bool Feature::getTransformationByRANSAC(Eigen::Matrix4f &result_transform, Eigen::Matrix<double, 6, 6> &result_information, int &point_count, int &point_corr_count, float &rmse, vector<cv::DMatch> *matches, // output vars. if matches == nullptr, do not return inlier match const Feature* earlier, const Feature* now, PointCloudCuda *pcc, const vector<cv::DMatch> &initial_matches) { if (matches != nullptr) matches->clear(); if (initial_matches.size() < (unsigned int) Config::instance()->get<int>("min_matches")) { return false; } unsigned int min_inlier_threshold = (unsigned int)(initial_matches.size() * Config::instance()->get<float>("min_inliers_percent")); std::vector<cv::DMatch> inlier; //holds those feature correspondences that support the transformation float inlier_error; //all squared errors srand((long)std::clock()); // a point is an inlier if it's no more than max_dist_m m from its partner apart const float max_dist_m = Config::instance()->get<float>("max_dist_for_inliers"); const float squared_max_dist_m = max_dist_m * max_dist_m; // best values of all iterations (including invalids) float best_error = 1e6, best_error_invalid = 1e6; unsigned int best_inlier_invalid = 0, best_inlier_cnt = 0, valid_iterations = 0; Eigen::Matrix4f transformation; const unsigned int sample_size = 3;// chose this many randomly from the correspondences: unsigned int ransac_iterations = Config::instance()->get<int>("ransac_max_iteration"); int threads = Config::instance()->get<int>("icpcuda_threads"); int blocks = Config::instance()->get<int>("icpcuda_blocks"); float corr_percent = Config::instance()->get<float>("coresp_percent"); Eigen::Matrix<double, 6, 6> information = Eigen::Matrix<double, 6, 6>::Identity(); int pc = 1.0, pcorrc = 0.0; bool *used = new bool[initial_matches.size()]; memset(used, 0, initial_matches.size() * sizeof(bool)); vector<int> sample_matches_indices(3); vector<cv::DMatch> sample_matches_vector(3); for (unsigned int n_iter = 0; n_iter < ransac_iterations; n_iter++) { for (int i = 0; i < sample_matches_indices.size(); i++) { used[sample_matches_indices[i]] = false; } sample_matches_indices.clear(); sample_matches_vector.clear(); while (sample_matches_indices.size() < sample_size) { int id = rand() % initial_matches.size(); if (!used[id]) { used[id] = true; sample_matches_indices.push_back(id); sample_matches_vector.push_back(initial_matches.at(id)); } } bool valid; // valid is false iff the sampled points clearly aren't inliers themself transformation = Feature::getTransformFromMatches(valid, earlier->feature_pts_3d, now->feature_pts_3d, sample_matches_vector, max_dist_m); if (!valid) continue; // valid is false iff the sampled points aren't inliers themself if (transformation != transformation) continue; //Contains NaN //test whether samples are inliers (more strict than before) Feature::computeInliersAndError(inlier, inlier_error, nullptr, sample_matches_vector, transformation, earlier->feature_pts_3d, now->feature_pts_3d, squared_max_dist_m); if (inlier_error > 1000) continue; //most possibly a false match in the samples Feature::computeInliersAndError(inlier, inlier_error, nullptr, initial_matches, transformation, earlier->feature_pts_3d, now->feature_pts_3d, squared_max_dist_m); if (inlier.size() < min_inlier_threshold || inlier_error > max_dist_m) { continue; } valid_iterations++; //Performance hacks: ///Iterations with more than half of the initial_matches inlying, count twice if (inlier.size() > initial_matches.size() * 0.5) n_iter++; ///Iterations with more than 80% of the initial_matches inlying, count threefold if (inlier.size() > initial_matches.size() * 0.8) n_iter++; if (inlier_error < best_error) { //copy this to the result if (pcc != nullptr) { Eigen::Vector3f t = transformation.topRightCorner(3, 1); Eigen::Matrix<float, 3, 3, Eigen::RowMajor> rot = transformation.topLeftCorner(3, 3); pcc->getCoresp(t, rot, information, pc, pcorrc, threads, blocks); } result_transform = transformation; result_information = information; point_count = pc; point_corr_count = pcorrc; if (matches != nullptr) *matches = inlier; best_inlier_cnt = inlier.size(); rmse = inlier_error; best_error = inlier_error; } float new_inlier_error; transformation = Feature::getTransformFromMatches(valid, earlier->feature_pts_3d, now->feature_pts_3d, inlier); // compute new trafo from all inliers: if (transformation != transformation) continue; //Contains NaN Feature::computeInliersAndError(inlier, new_inlier_error, nullptr, initial_matches, transformation, earlier->feature_pts_3d, now->feature_pts_3d, squared_max_dist_m); if(inlier.size() < min_inlier_threshold || new_inlier_error > max_dist_m) { continue; } if (new_inlier_error < best_error) { if (pcc != nullptr) { Eigen::Vector3f t = transformation.topRightCorner(3, 1); Eigen::Matrix<float, 3, 3, Eigen::RowMajor> rot = transformation.topLeftCorner(3, 3); pcc->getCoresp(t, rot, information, pc, pcorrc, threads, blocks); } result_transform = transformation; if (pcc != nullptr) result_information = information; point_count = pc; point_corr_count = pcorrc; if (matches != nullptr) *matches = inlier; best_inlier_cnt = inlier.size(); rmse = new_inlier_error; best_error = new_inlier_error; } } //iterations if (pcc == nullptr) result_information = Eigen::Matrix<double, 6, 6>::Identity(); return best_inlier_cnt >= min_inlier_threshold; }
void MainController::run() { while(!pangolin::ShouldQuit() && !((!logReader->hasMore()) && quiet) && !(eFusion->getTick() == end && quiet)) { if(!gui->pause->Get() || pangolin::Pushed(*gui->step)) { if((logReader->hasMore() || rewind) && eFusion->getTick() < end) { TICK("LogRead"); if(rewind) { if(!logReader->hasMore()) { logReader->getBack(); } else { logReader->getNext(); } if(logReader->rewound()) { logReader->currentFrame = 0; } } else { logReader->getNext(); } TOCK("LogRead"); if(eFusion->getTick() < start) { eFusion->setTick(start); logReader->fastForward(start); } float weightMultiplier = framesToSkip + 1; if(framesToSkip > 0) { eFusion->setTick(eFusion->getTick() + framesToSkip); logReader->fastForward(logReader->currentFrame + framesToSkip); framesToSkip = 0; } Eigen::Matrix4f * currentPose = 0; if(groundTruthOdometry) { currentPose = new Eigen::Matrix4f; currentPose->setIdentity(); *currentPose = groundTruthOdometry->getIncrementalTransformation(logReader->timestamp); } eFusion->processFrame(logReader->rgb, logReader->depth, logReader->timestamp, currentPose, weightMultiplier); if(currentPose) { delete currentPose; } if(frameskip && Stopwatch::getInstance().getTimings().at("Run") > 1000.f / 30.f) { framesToSkip = int(Stopwatch::getInstance().getTimings().at("Run") / (1000.f / 30.f)); } } } else { eFusion->predict(); } TICK("GUI"); if(gui->followPose->Get()) { pangolin::OpenGlMatrix mv; Eigen::Matrix4f currPose = eFusion->getCurrPose(); Eigen::Matrix3f currRot = currPose.topLeftCorner(3, 3); Eigen::Quaternionf currQuat(currRot); Eigen::Vector3f forwardVector(0, 0, 1); Eigen::Vector3f upVector(0, iclnuim ? 1 : -1, 0); Eigen::Vector3f forward = (currQuat * forwardVector).normalized(); Eigen::Vector3f up = (currQuat * upVector).normalized(); Eigen::Vector3f eye(currPose(0, 3), currPose(1, 3), currPose(2, 3)); eye -= forward; Eigen::Vector3f at = eye + forward; Eigen::Vector3f z = (eye - at).normalized(); // Forward Eigen::Vector3f x = up.cross(z).normalized(); // Right Eigen::Vector3f y = z.cross(x); Eigen::Matrix4d m; m << x(0), x(1), x(2), -(x.dot(eye)), y(0), y(1), y(2), -(y.dot(eye)), z(0), z(1), z(2), -(z.dot(eye)), 0, 0, 0, 1; memcpy(&mv.m[0], m.data(), sizeof(Eigen::Matrix4d)); gui->s_cam.SetModelViewMatrix(mv); } gui->preCall(); std::stringstream stri; stri << eFusion->getModelToModel().lastICPCount; gui->trackInliers->Ref().Set(stri.str()); std::stringstream stre; stre << (isnan(eFusion->getModelToModel().lastICPError) ? 0 : eFusion->getModelToModel().lastICPError); gui->trackRes->Ref().Set(stre.str()); if(!gui->pause->Get()) { gui->resLog.Log((isnan(eFusion->getModelToModel().lastICPError) ? std::numeric_limits<float>::max() : eFusion->getModelToModel().lastICPError), icpErrThresh); gui->inLog.Log(eFusion->getModelToModel().lastICPCount, icpCountThresh); } Eigen::Matrix4f pose = eFusion->getCurrPose(); if(gui->drawRawCloud->Get() || gui->drawFilteredCloud->Get()) { eFusion->computeFeedbackBuffers(); } if(gui->drawRawCloud->Get()) { eFusion->getFeedbackBuffers().at(FeedbackBuffer::RAW)->render(gui->s_cam.GetProjectionModelViewMatrix(), pose, gui->drawNormals->Get(), gui->drawColors->Get()); } if(gui->drawFilteredCloud->Get()) { eFusion->getFeedbackBuffers().at(FeedbackBuffer::FILTERED)->render(gui->s_cam.GetProjectionModelViewMatrix(), pose, gui->drawNormals->Get(), gui->drawColors->Get()); } if(gui->drawGlobalModel->Get()) { glFinish(); TICK("Global"); if(gui->drawFxaa->Get()) { gui->drawFXAA(gui->s_cam.GetProjectionModelViewMatrix(), gui->s_cam.GetModelViewMatrix(), eFusion->getGlobalModel().model(), eFusion->getConfidenceThreshold(), eFusion->getTick(), eFusion->getTimeDelta(), iclnuim); } else { eFusion->getGlobalModel().renderPointCloud(gui->s_cam.GetProjectionModelViewMatrix(), eFusion->getConfidenceThreshold(), gui->drawUnstable->Get(), gui->drawNormals->Get(), gui->drawColors->Get(), gui->drawPoints->Get(), gui->drawWindow->Get(), gui->drawTimes->Get(), eFusion->getTick(), eFusion->getTimeDelta()); } glFinish(); TOCK("Global"); } if(eFusion->getLost()) { glColor3f(1, 1, 0); } else { glColor3f(1, 0, 1); } gui->drawFrustum(pose); glColor3f(1, 1, 1); if(gui->drawFerns->Get()) { glColor3f(0, 0, 0); for(size_t i = 0; i < eFusion->getFerns().frames.size(); i++) { if((int)i == eFusion->getFerns().lastClosest) continue; gui->drawFrustum(eFusion->getFerns().frames.at(i)->pose); } glColor3f(1, 1, 1); } if(gui->drawDefGraph->Get()) { const std::vector<GraphNode*> & graph = eFusion->getLocalDeformation().getGraph(); for(size_t i = 0; i < graph.size(); i++) { pangolin::glDrawCross(graph.at(i)->position(0), graph.at(i)->position(1), graph.at(i)->position(2), 0.1); for(size_t j = 0; j < graph.at(i)->neighbours.size(); j++) { pangolin::glDrawLine(graph.at(i)->position(0), graph.at(i)->position(1), graph.at(i)->position(2), graph.at(graph.at(i)->neighbours.at(j))->position(0), graph.at(graph.at(i)->neighbours.at(j))->position(1), graph.at(graph.at(i)->neighbours.at(j))->position(2)); } } } if(eFusion->getFerns().lastClosest != -1) { glColor3f(1, 0, 0); gui->drawFrustum(eFusion->getFerns().frames.at(eFusion->getFerns().lastClosest)->pose); glColor3f(1, 1, 1); } const std::vector<PoseMatch> & poseMatches = eFusion->getPoseMatches(); int maxDiff = 0; for(size_t i = 0; i < poseMatches.size(); i++) { if(poseMatches.at(i).secondId - poseMatches.at(i).firstId > maxDiff) { maxDiff = poseMatches.at(i).secondId - poseMatches.at(i).firstId; } } for(size_t i = 0; i < poseMatches.size(); i++) { if(gui->drawDeforms->Get()) { if(poseMatches.at(i).fern) { glColor3f(1, 0, 0); } else { glColor3f(0, 1, 0); } for(size_t j = 0; j < poseMatches.at(i).constraints.size(); j++) { pangolin::glDrawLine(poseMatches.at(i).constraints.at(j).sourcePoint(0), poseMatches.at(i).constraints.at(j).sourcePoint(1), poseMatches.at(i).constraints.at(j).sourcePoint(2), poseMatches.at(i).constraints.at(j).targetPoint(0), poseMatches.at(i).constraints.at(j).targetPoint(1), poseMatches.at(i).constraints.at(j).targetPoint(2)); } } } glColor3f(1, 1, 1); eFusion->normaliseDepth(0.3f, gui->depthCutoff->Get()); for(std::map<std::string, GPUTexture*>::const_iterator it = eFusion->getTextures().begin(); it != eFusion->getTextures().end(); ++it) { if(it->second->draw) { gui->displayImg(it->first, it->second); } } eFusion->getIndexMap().renderDepth(gui->depthCutoff->Get()); gui->displayImg("ModelImg", eFusion->getIndexMap().imageTex()); gui->displayImg("Model", eFusion->getIndexMap().drawTex()); std::stringstream strs; strs << eFusion->getGlobalModel().lastCount(); gui->totalPoints->operator=(strs.str()); std::stringstream strs2; strs2 << eFusion->getLocalDeformation().getGraph().size(); gui->totalNodes->operator=(strs2.str()); std::stringstream strs3; strs3 << eFusion->getFerns().frames.size(); gui->totalFerns->operator=(strs3.str()); std::stringstream strs4; strs4 << eFusion->getDeforms(); gui->totalDefs->operator=(strs4.str()); std::stringstream strs5; strs5 << eFusion->getTick() << "/" << logReader->getNumFrames(); gui->logProgress->operator=(strs5.str()); std::stringstream strs6; strs6 << eFusion->getFernDeforms(); gui->totalFernDefs->operator=(strs6.str()); gui->postCall(); logReader->flipColors = gui->flipColors->Get(); eFusion->setRgbOnly(gui->rgbOnly->Get()); eFusion->setPyramid(gui->pyramid->Get()); eFusion->setFastOdom(gui->fastOdom->Get()); eFusion->setConfidenceThreshold(gui->confidenceThreshold->Get()); eFusion->setDepthCutoff(gui->depthCutoff->Get()); eFusion->setIcpWeight(gui->icpWeight->Get()); eFusion->setSo3(gui->so3->Get()); eFusion->setFrameToFrameRGB(gui->frameToFrameRGB->Get()); resetButton = pangolin::Pushed(*gui->reset); if(gui->autoSettings) { static bool last = gui->autoSettings->Get(); if(gui->autoSettings->Get() != last) { last = gui->autoSettings->Get(); static_cast<LiveLogReader *>(logReader)->setAuto(last); } } Stopwatch::getInstance().sendAll(); if(resetButton) { break; } if(pangolin::Pushed(*gui->save)) { eFusion->savePly(); } TOCK("GUI"); } }
void DiriniVisualizer::addPoint(Eigen::Vector3f point, Eigen::Matrix4f transform_) { points.push_back(transform_.topLeftCorner(3,3)*point+transform_.topRightCorner(3,1)); }