template <typename PointSource, typename PointTarget> void pcl16::GeneralizedIterativeClosestPoint<PointSource, PointTarget>::estimateRigidTransformationBFGS (const PointCloudSource &cloud_src, const std::vector<int> &indices_src, const PointCloudTarget &cloud_tgt, const std::vector<int> &indices_tgt, Eigen::Matrix4f &transformation_matrix) { if (indices_src.size () < 4) // need at least 4 samples { PCL16_THROW_EXCEPTION (NotEnoughPointsException, "[pcl16::GeneralizedIterativeClosestPoint::estimateRigidTransformationBFGS] Need at least 4 points to estimate a transform! Source and target have " << indices_src.size () << " points!"); return; } // Set the initial solution Vector6d x = Vector6d::Zero (); x[0] = transformation_matrix (0,3); x[1] = transformation_matrix (1,3); x[2] = transformation_matrix (2,3); x[3] = atan2 (transformation_matrix (2,1), transformation_matrix (2,2)); x[4] = asin (-transformation_matrix (2,0)); x[5] = atan2 (transformation_matrix (1,0), transformation_matrix (0,0)); // Set temporary pointers tmp_src_ = &cloud_src; tmp_tgt_ = &cloud_tgt; tmp_idx_src_ = &indices_src; tmp_idx_tgt_ = &indices_tgt; // Optimize using forward-difference approximation LM const double gradient_tol = 1e-2; OptimizationFunctorWithIndices functor(this); BFGS<OptimizationFunctorWithIndices> bfgs (functor); bfgs.parameters.sigma = 0.01; bfgs.parameters.rho = 0.01; bfgs.parameters.tau1 = 9; bfgs.parameters.tau2 = 0.05; bfgs.parameters.tau3 = 0.5; bfgs.parameters.order = 3; int inner_iterations_ = 0; int result = bfgs.minimizeInit (x); do { inner_iterations_++; result = bfgs.minimizeOneStep (x); if(result) { break; } result = bfgs.testGradient(gradient_tol); } while(result == BFGSSpace::Running && inner_iterations_ < max_inner_iterations_); if(result == BFGSSpace::NoProgress || result == BFGSSpace::Success || inner_iterations_ == max_inner_iterations_) { PCL16_DEBUG ("[pcl16::registration::TransformationEstimationBFGS::estimateRigidTransformation]"); PCL16_DEBUG ("BFGS solver finished with exit code %i \n", result); transformation_matrix.setIdentity(); applyState(transformation_matrix, x); } else PCL16_THROW_EXCEPTION(SolverDidntConvergeException, "[pcl16::" << getClassName () << "::TransformationEstimationBFGS::estimateRigidTransformation] BFGS solver didn't converge!"); }
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"); } }
/* ---[ */ 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 sensor_msgs::PointCloud2::Ptr cloud (new sensor_msgs::PointCloud2); if (!loadCloud (argv[p_file_indices[0]], *cloud)) return (-1); // Apply the transform sensor_msgs::PointCloud2 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); }