i/rtabmap
1
0
Fork 0
mirror of https://github.com/introlab/rtabmap.git synced 2026-09-02 17:40:23 +08:00
Code Issues Packages Projects Releases Wiki Activity

Added OptimizerCeres class (Optimizer/Strategy=3)

Browse Source
This commit is contained in:
matlabbe
2019-08-17 03:55:04 -04:00
parent 38044040a6
commit 536f001479
29 changed files with 1982 additions and 284 deletions
Download Patch File Download Diff File Expand all files Collapse all files

511
corelib/src/optimizer/OptimizerCeres.cpp Normal file
View File

@@ -0,0 +1,511 @@
/*
Copyright (c) 2010-2019, Mathieu Labbe - IntRoLab - Universite de Sherbrooke
All rights reserved.
Redistribution and use in source and binary forms, with or without
modification, are permitted provided that the following conditions are met:
* Redistributions of source code must retain the above copyright
notice, this list of conditions and the following disclaimer.
* Redistributions in binary form must reproduce the above copyright
notice, this list of conditions and the following disclaimer in the
documentation and/or other materials provided with the distribution.
* Neither the name of the Universite de Sherbrooke nor the
names of its contributors may be used to endorse or promote products
derived from this software without specific prior written permission.
THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY
DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
(INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
(INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
*/
#include "rtabmap/core/Graph.h"
#include <rtabmap/utilite/ULogger.h>
#include <rtabmap/utilite/UStl.h>
#include <rtabmap/utilite/UMath.h>
#include <rtabmap/utilite/UConversion.h>
#include <rtabmap/utilite/UTimer.h>
#include <set>
#include <rtabmap/core/optimizer/OptimizerCeres.h>
#ifdef RTABMAP_CERES
#include <ceres/ceres.h>
#include <ceres/local_parameterization.h>
#include "ceres/pose_graph_2d/types.h"
#include "ceres/pose_graph_2d/pose_graph_2d_error_term.h"
#include "ceres/pose_graph_2d/angle_local_parameterization.h"
#include "ceres/pose_graph_3d/types.h"
#include "ceres/pose_graph_3d/pose_graph_3d_error_term.h"
#include "ceres/bundle/BAProblem.h"
#include "ceres/bundle/snavely_reprojection_error.h"
#if not(CERES_VERSION_MAJOR > 1 || (CERES_VERSION_MAJOR == 1 && CERES_VERSION_MINOR >= 12))
#include "ceres/pose_graph_3d/eigen_quaternion_parameterization.h"
#endif
#endif
namespace rtabmap {
bool OptimizerCeres::available()
{
#ifdef RTABMAP_CERES
return true;
#else
return false;
#endif
}
std::map<int, Transform> OptimizerCeres::optimize(
int rootId,
const std::map<int, Transform> & poses,
const std::multimap<int, Link> & edgeConstraints,
cv::Mat & outputCovariance,
std::list<std::map<int, Transform> > * intermediateGraphes, // contains poses after tree init to last one before the end
double * finalError,
int * iterationsDone)
{
outputCovariance = cv::Mat::eye(6,6,CV_64FC1);
std::map<int, Transform> optimizedPoses;
#ifdef RTABMAP_CERES
UDEBUG("Optimizing graph (pose=%d constraints=%d)...", (int)poses.size(), (int)edgeConstraints.size());
if(edgeConstraints.size()>=1 && poses.size()>=2 && iterations() > 0)
{
//Build problem
ceres::Problem problem;
std::map<int, ceres::examples::Pose2d> poses2d;
ceres::examples::MapOfPoses poses3d;
UDEBUG("fill poses to Ceres...");
if(isSlam2d())
{
for(std::map<int, Transform>::const_iterator iter = poses.begin(); iter!=poses.end(); ++iter)
{
if(iter->first > 0)
{
UASSERT(!iter->second.isNull());
ceres::examples::Pose2d p;
p.x = iter->second.x();
p.y = iter->second.y();
p.yaw_radians = ceres::examples::NormalizeAngle(iter->second.theta());
poses2d.insert(std::make_pair(iter->first, p));
}
}
}
else
{
for(std::map<int, Transform>::const_iterator iter = poses.begin(); iter!=poses.end(); ++iter)
{
if(iter->first > 0)
{
UASSERT(!iter->second.isNull());
ceres::examples::Pose3d p;
p.p.x() = iter->second.x();
p.p.y() = iter->second.y();
p.p.z() = iter->second.z();
p.q = iter->second.getQuaterniond();
poses3d.insert(std::make_pair(iter->first, p));
}
}
}
ceres::LossFunction* loss_function = NULL;
ceres::LocalParameterization* angle_local_parameterization = NULL;
ceres::LocalParameterization* quaternion_local_parameterization = NULL;
for(std::multimap<int, Link>::const_iterator iter=edgeConstraints.begin(); iter!=edgeConstraints.end(); ++iter)
{
int id1 = iter->second.from();
int id2 = iter->second.to();
if(id1 != id2 && id1 > 0 && id2 > 0)
{
UASSERT(poses.find(id1) != poses.end() && poses.find(id2) != poses.end());
if(isSlam2d())
{
Eigen::Matrix<double, 3, 3> information = Eigen::Matrix<double, 3, 3>::Identity();
if(!isCovarianceIgnored())
{
information(0,0) = iter->second.infMatrix().at<double>(0,0); // x-x
information(0,1) = iter->second.infMatrix().at<double>(0,1); // x-y
information(0,2) = iter->second.infMatrix().at<double>(0,5); // x-theta
information(1,0) = iter->second.infMatrix().at<double>(1,0); // y-x
information(1,1) = iter->second.infMatrix().at<double>(1,1); // y-y
information(1,2) = iter->second.infMatrix().at<double>(1,5); // y-theta
information(2,0) = iter->second.infMatrix().at<double>(5,0); // theta-x
information(2,1) = iter->second.infMatrix().at<double>(5,1); // theta-y
information(2,2) = iter->second.infMatrix().at<double>(5,5); // theta-theta
}
float yaw_radians = ceres::examples::NormalizeAngle(iter->second.transform().theta());
const Eigen::Matrix3d sqrt_information = information.llt().matrixL();
// Ceres will take ownership of the pointer.
ceres::CostFunction* cost_function = ceres::examples::PoseGraph2dErrorTerm::Create(
iter->second.transform().x(),
iter->second.transform().y(),
yaw_radians,
sqrt_information);
std::map<int, ceres::examples::Pose2d>::iterator pose_begin_iter = poses2d.find(id1);
std::map<int, ceres::examples::Pose2d>::iterator pose_end_iter = poses2d.find(id2);
problem.AddResidualBlock(
cost_function, loss_function,
&pose_begin_iter->second.x, &pose_begin_iter->second.y, &pose_begin_iter->second.yaw_radians,
&pose_end_iter->second.x, &pose_end_iter->second.y, &pose_end_iter->second.yaw_radians);
if(angle_local_parameterization == NULL)
{
angle_local_parameterization = ceres::examples::AngleLocalParameterization::Create();
}
problem.SetParameterization(&pose_begin_iter->second.yaw_radians, angle_local_parameterization);
problem.SetParameterization(&pose_end_iter->second.yaw_radians, angle_local_parameterization);
}
else
{
ceres::examples::MapOfPoses::iterator pose_begin_iter = poses3d.find(id1);
ceres::examples::MapOfPoses::iterator pose_end_iter = poses3d.find(id2);
ceres::examples::Constraint3d constraint;
Eigen::Matrix<double, 6, 6> information = Eigen::Matrix<double, 6, 6>::Identity();
if(!isCovarianceIgnored())
{
memcpy(information.data(), iter->second.infMatrix().data, iter->second.infMatrix().total()*sizeof(double));
}
ceres::examples::Pose3d t;
t.p.x() = iter->second.transform().x();
t.p.y() = iter->second.transform().y();
t.p.z() = iter->second.transform().z();
t.q = iter->second.transform().getQuaterniond();
const Eigen::Matrix<double, 6, 6> sqrt_information = information.llt().matrixL();
// Ceres will take ownership of the pointer.
ceres::CostFunction* cost_function = ceres::examples::PoseGraph3dErrorTerm::Create(t, sqrt_information);
problem.AddResidualBlock(cost_function, loss_function,
pose_begin_iter->second.p.data(), pose_begin_iter->second.q.coeffs().data(),
pose_end_iter->second.p.data(), pose_end_iter->second.q.coeffs().data());
if(quaternion_local_parameterization == NULL)
{
quaternion_local_parameterization = new ceres::EigenQuaternionParameterization;
}
problem.SetParameterization(pose_begin_iter->second.q.coeffs().data(), quaternion_local_parameterization);
problem.SetParameterization(pose_end_iter->second.q.coeffs().data(), quaternion_local_parameterization);
}
}
//else // not supporting pose prior and landmarks
}
if(isSlam2d())
{
// The pose graph optimization problem has three DOFs that are not fully
// constrained. This is typically referred to as gauge freedom. You can apply
// a rigid body transformation to all the nodes and the optimization problem
// will still have the exact same cost. The Levenberg-Marquardt algorithm has
// internal damping which mitigate this issue, but it is better to properly
// constrain the gauge freedom. This can be done by setting one of the poses
// as constant so the optimizer cannot change it.
std::map<int, ceres::examples::Pose2d>::iterator pose_start_iter = rootId>0?poses2d.find(rootId):poses2d.begin();
UASSERT(pose_start_iter != poses2d.end());
problem.SetParameterBlockConstant(&pose_start_iter->second.x);
problem.SetParameterBlockConstant(&pose_start_iter->second.y);
problem.SetParameterBlockConstant(&pose_start_iter->second.yaw_radians);
}
else
{
// The pose graph optimization problem has six DOFs that are not fully
// constrained. This is typically referred to as gauge freedom. You can apply
// a rigid body transformation to all the nodes and the optimization problem
// will still have the exact same cost. The Levenberg-Marquardt algorithm has
// internal damping which mitigates this issue, but it is better to properly
// constrain the gauge freedom. This can be done by setting one of the poses
// as constant so the optimizer cannot change it.
ceres::examples::MapOfPoses::iterator pose_start_iter = rootId>0?poses3d.find(rootId):poses3d.begin();
UASSERT(pose_start_iter != poses3d.end());
problem.SetParameterBlockConstant(pose_start_iter->second.p.data());
problem.SetParameterBlockConstant(pose_start_iter->second.q.coeffs().data());
}
UINFO("Ceres optimizing begin (iterations=%d)", iterations());
ceres::Solver::Options options;
options.max_num_iterations = iterations();
options.linear_solver_type = ceres::SPARSE_NORMAL_CHOLESKY;
options.function_tolerance = this->epsilon();
ceres::Solver::Summary summary;
UTimer timer;
ceres::Solve(options, &problem, &summary);
if(ULogger::level() == ULogger::kDebug)
{
UDEBUG("Ceres Report:");
std::cout << summary.FullReport() << '\n';
}
if(!summary.IsSolutionUsable())
{
UWARN("ceres: Could not find a usable solution, aborting optimization!");
return optimizedPoses;
}
if(finalError)
{
*finalError = summary.final_cost;
}
if(iterationsDone)
{
*iterationsDone = summary.iterations.size();
}
UINFO("Ceres optimizing end (%d iterations done, error=%f, time = %f s)", (int)summary.iterations.size(), summary.final_cost, timer.ticks());
for(std::map<int, Transform>::const_iterator iter = poses.begin(); iter!=poses.end(); ++iter)
{
if(iter->first > 0)
{
if(isSlam2d())
{
const std::map<int, ceres::examples::Pose2d>::iterator & pter = poses2d.find(iter->first);
float roll, pitch, yaw;
iter->second.getEulerAngles(roll, pitch, yaw);
Transform newPose(pter->second.x, pter->second.y, iter->second.z(), roll, pitch, pter->second.yaw_radians);
UASSERT_MSG(!newPose.isNull(), uFormat("Optimized pose %d is null!?!?", iter->first).c_str());
optimizedPoses.insert(std::pair<int, Transform>(iter->first, newPose));
}
else
{
const std::map<int, ceres::examples::Pose3d, std::less<int>,
Eigen::aligned_allocator<std::pair<const int, ceres::examples::Pose3d> > >::
iterator& pter = poses3d.find(iter->first);
Transform newPose(pter->second.p.x(), pter->second.p.y(), pter->second.p.z(), pter->second.q.x(), pter->second.q.y(), pter->second.q.z(), pter->second.q.w());
UASSERT_MSG(!newPose.isNull(), uFormat("Optimized pose %d is null!?!?", iter->first).c_str());
optimizedPoses.insert(std::pair<int, Transform>(iter->first, newPose));
}
}
}
// Ceres doesn't compute marginals...
}
else if(poses.size() == 1 || iterations() <= 0)
{
optimizedPoses = poses;
}
else
{
UWARN("This method should be called at least with 1 pose!");
}
UDEBUG("Optimizing graph...end!");
#else
UERROR("Not built with Ceres support!");
#endif
return optimizedPoses;
}
std::map<int, Transform> OptimizerCeres::optimizeBA(
int rootId,
const std::map<int, Transform> & posesIn,
const std::multimap<int, Link> & links,
const std::map<int, CameraModel> & models,
std::map<int, cv::Point3f> & points3DMap,
const std::map<int, std::map<int, FeatureBA> > & wordReferences, // <ID words, IDs frames + keypoint/Disparity>)
std::set<int> * outliers)
{
#ifdef RTABMAP_CERES
// run sba optimization
std::map<int, Transform> poses(posesIn.lower_bound(1), posesIn.end());
ceres::BAProblem baProblem;
baProblem.num_cameras_ = poses.size();
baProblem.num_points_ = points3DMap.size();
baProblem.num_observations_ = 0;
for(std::map<int, std::map<int, FeatureBA> >::const_iterator iter=wordReferences.begin();
iter!=wordReferences.end();
++iter)
{
baProblem.num_observations_ += iter->second.size();
}
baProblem.point_index_ = new int[baProblem.num_observations_];
baProblem.camera_index_ = new int[baProblem.num_observations_];
baProblem.observations_ = new double[4 * baProblem.num_observations_];
baProblem.cameras_ = new double[6 * baProblem.num_cameras_];
baProblem.points_ = new double[3 * baProblem.num_points_];
// Each camera is a set of 6 parameters: R and t. The rotation R is specified as a Rodrigues' vector.
int oi=0;
int camIndex=0;
std::map<int, int> camIdToIndex;
for(std::map<int, Transform>::const_iterator iter=poses.begin();
iter!=poses.end();
++iter)
{
// Get camera model
std::map<int, CameraModel>::const_iterator iterModel = models.find(iter->first);
UASSERT(iterModel != models.end() && iterModel->second.isValidForProjection());
const Transform & t = (iter->second * iterModel->second.localTransform()).inverse();
cv::Mat R = (cv::Mat_<double>(3,3) <<
(double)t.r11(), (double)t.r12(), (double)t.r13(),
(double)t.r21(), (double)t.r22(), (double)t.r23(),
(double)t.r31(), (double)t.r32(), (double)t.r33());
cv::Mat rvec(1,3, CV_64FC1);
cv::Rodrigues(R, rvec);
UASSERT(oi+6 <= baProblem.num_cameras_*6);
baProblem.cameras_[oi++] = rvec.at<double>(0,0);
baProblem.cameras_[oi++] = rvec.at<double>(0,1);
baProblem.cameras_[oi++] = rvec.at<double>(0,2);
baProblem.cameras_[oi++] = t.x();
baProblem.cameras_[oi++] = t.y();
baProblem.cameras_[oi++] = t.z();
camIdToIndex.insert(std::make_pair(iter->first, camIndex++));
}
UASSERT(oi == baProblem.num_cameras_*6);
oi=0;
int pointIndex=0;
std::map<int, int> pointIdToIndex;
for(std::map<int, cv::Point3f>::const_iterator kter = points3DMap.begin(); kter!=points3DMap.end(); ++kter)
{
UASSERT(oi+3 <= baProblem.num_points_*3);
baProblem.points_[oi++] = kter->second.x;
baProblem.points_[oi++] = kter->second.y;
baProblem.points_[oi++] = kter->second.z;
pointIdToIndex.insert(std::make_pair(kter->first, pointIndex++));
}
UASSERT(oi == baProblem.num_points_*3);
oi = 0;
for(std::map<int, std::map<int, FeatureBA> >::const_iterator iter=wordReferences.begin();
iter!=wordReferences.end();
++iter)
{
for(std::map<int, FeatureBA>::const_iterator jter=iter->second.begin();
jter!=iter->second.end();
++jter)
{
std::map<int, CameraModel>::const_iterator iterModel = models.find(jter->first);
UASSERT(iterModel != models.end() && iterModel->second.isValidForProjection());
baProblem.camera_index_[oi] = camIdToIndex.at(jter->first);
baProblem.point_index_[oi] = pointIdToIndex.at(iter->first);
baProblem.observations_[4*oi] = jter->second.kpt.pt.x - iterModel->second.cx();
baProblem.observations_[4*oi+1] = jter->second.kpt.pt.y - iterModel->second.cy();
baProblem.observations_[4*oi+2] = iterModel->second.fx();
baProblem.observations_[4*oi+3] = iterModel->second.fy();
++oi;
}
}
UASSERT(oi == baProblem.num_observations_);
// Build problem
const double* observations = baProblem.observations();
// Create residuals for each observation in the bundle adjustment problem. The
// parameters for cameras and points are added automatically.
ceres::Problem problem;
for (int i = 0; i < baProblem.num_observations(); ++i) {
// Each Residual block takes a point and a camera as input and outputs a 2
// dimensional residual. Internally, the cost function stores the observed
// image location and compares the reprojection against the observation.
ceres::CostFunction* cost_function =
ceres::SnavelyReprojectionError::Create(
observations[4 * i], //u
observations[4 * i + 1], //v
observations[4 * i + 2], //fx
observations[4 * i + 3]); //fy
ceres::LossFunction* loss_function = new ceres::HuberLoss(8.0);
problem.AddResidualBlock(cost_function,
loss_function,
baProblem.mutable_camera_for_observation(i),
baProblem.mutable_point_for_observation(i));
}
// SBA
// Make Ceres automatically detect the bundle structure. Note that the
// standard solver, SPARSE_NORMAL_CHOLESKY, also works fine but it is slower
// for standard bundle adjustment problems.
ceres::Solver::Options options;
options.linear_solver_type = ceres::DENSE_SCHUR;
options.max_num_iterations = iterations();
//options.linear_solver_type = ceres::SPARSE_NORMAL_CHOLESKY;
options.function_tolerance = this->epsilon();
ceres::Solver::Summary summary;
ceres::Solve(options, &problem, &summary);
if(ULogger::level() == ULogger::kDebug)
{
UDEBUG("Ceres report:");
std::cout << summary.FullReport() << "\n";
}
if(!summary.IsSolutionUsable())
{
UWARN("ceres: Could not find a usable solution, aborting optimization!");
return poses;
}
//update poses
std::map<int, Transform> newPoses = poses;
oi=0;
for(std::map<int, Transform>::iterator iter=newPoses.begin(); iter!=newPoses.end(); ++iter)
{
cv::Mat rvec = (cv::Mat_<double>(1,3) <<
baProblem.cameras_[oi], baProblem.cameras_[oi+1], baProblem.cameras_[oi+2]);
cv::Mat R;
cv::Rodrigues(rvec, R);
Transform t(R.at<double>(0,0), R.at<double>(0,1), R.at<double>(0,2), baProblem.cameras_[oi+3],
R.at<double>(1,0), R.at<double>(1,1), R.at<double>(1,2), baProblem.cameras_[oi+4],
R.at<double>(2,0), R.at<double>(2,1), R.at<double>(2,2), baProblem.cameras_[oi+5]);
oi+=6;
if(this->isSlam2d())
{
t = (models.at(iter->first).localTransform() * t).inverse();
t = iter->second.inverse() * t;
iter->second *= t.to3DoF();
}
else
{
iter->second = (models.at(iter->first).localTransform() * t).inverse();
}
}
//update 3D points
oi = 0;
for(std::map<int, cv::Point3f>::iterator kter = points3DMap.begin(); kter!=points3DMap.end(); ++kter)
{
kter->second.x = baProblem.points_[oi++];
kter->second.y = baProblem.points_[oi++];
kter->second.z = baProblem.points_[oi++];
}
return newPoses;
#else
UERROR("RTAB-Map is not built with ceres!");
return std::map<int, Transform>();
#endif
}
} /* namespace rtabmap */

6
corelib/src/optimizer/OptimizerG2O.cpp
View File

@@ -1372,7 +1372,7 @@ std::map<int, Transform> OptimizerG2O::optimizeBA(
// negative root means that all other poses should be fixed instead of the root
vCam->setFixed((rootId >= 0 && iter->first == rootId) || (rootId < 0 && iter->first != -rootId));
UDEBUG("cam %d (fixed=%d) fx=%f fy=%f cx=%f cy=%f Tx=%f baseline=%f t=%s",
/*UDEBUG("cam %d (fixed=%d) fx=%f fy=%f cx=%f cy=%f Tx=%f baseline=%f t=%s",
iter->first,
vCam->fixed()?1:0,
iterModel->second.fx(),
@@ -1381,7 +1381,7 @@ std::map<int, Transform> OptimizerG2O::optimizeBA(
iterModel->second.cy(),
iterModel->second.Tx(),
iterModel->second.Tx()<0.0?-iterModel->second.Tx()/iterModel->second.fx():baseline_,
camPose.prettyPrint().c_str());
camPose.prettyPrint().c_str());*/
UASSERT_MSG(optimizer.addVertex(vCam), uFormat("cannot insert vertex %d!?", iter->first).c_str());
}
@@ -1721,7 +1721,7 @@ std::map<int, Transform> OptimizerG2O::optimizeBA(
// remove model local transform
t *= models.at(iter->first).localTransform().inverse();
UDEBUG("%d from=%s to=%s", iter->first, iter->second.prettyPrint().c_str(), t.prettyPrint().c_str());
//UDEBUG("%d from=%s to=%s", iter->first, iter->second.prettyPrint().c_str(), t.prettyPrint().c_str());
if(t.isNull())
{
UERROR("Optimized pose %d is null!?!?", iter->first);

52
corelib/src/optimizer/ceres/bundle/BAProblem.h Normal file
View File

@@ -0,0 +1,52 @@
/*
* BAProblem.h
*
* Created on: Aug 16, 2019
* Author: mathieu
*/
#ifndef CORELIB_SRC_OPTIMIZER_CERES_BUNDLE_BAPROBLEM_H_
#define CORELIB_SRC_OPTIMIZER_CERES_BUNDLE_BAPROBLEM_H_
namespace ceres {
class BAProblem {
public:
BAProblem() :
num_cameras_(0),
num_points_(0),
num_observations_(0),
point_index_(NULL),
camera_index_(NULL),
observations_(NULL),
cameras_(NULL),
points_(NULL)
{}
~BAProblem() {
delete[] point_index_;
delete[] camera_index_;
delete[] observations_;
delete[] cameras_;
delete[] points_;
}
int num_observations() const { return num_observations_;}
const double* observations() const { return observations_;}
double* mutable_camera_for_observation(int i) {
return cameras_ + camera_index_[i] * 6;
}
double* mutable_point_for_observation(int i) {
return points_ + point_index_[i] * 3;
}
int num_cameras_;
int num_points_;
int num_observations_;
int* point_index_;
int* camera_index_;
double* observations_;
double* cameras_;
double* points_;
};
}
#endif /* CORELIB_SRC_OPTIMIZER_CERES_BUNDLE_BAPROBLEM_H_ */

69
corelib/src/optimizer/ceres/bundle/snavely_reprojection_error.h Normal file
View File

@@ -0,0 +1,69 @@
/*
* snavely_reprojection_error.h
*
* Created on: Aug 16, 2019
* Author: mathieu
*/
#ifndef CORELIB_SRC_OPTIMIZER_CERES_BUNDLE_SNAVELY_REPROJECTION_ERROR_H_
#define CORELIB_SRC_OPTIMIZER_CERES_BUNDLE_SNAVELY_REPROJECTION_ERROR_H_
#include "ceres/ceres.h"
#include "ceres/rotation.h"
#include <rtabmap/utilite/ULogger.h>
namespace ceres {
// Templated pinhole camera model for used with Ceres. The camera is
// parameterized using 6 parameters: 3 for rotation, 3 for translation. The principal point is not modeled
// (i.e. it is assumed be located at the image center).
struct SnavelyReprojectionError {
SnavelyReprojectionError(double observed_x, double observed_y, double fx, double fy)
: observed_x(observed_x), observed_y(observed_y), fx(fx), fy(fy) {}
template <typename T>
bool operator()(const T* const camera,
const T* const point,
T* residuals) const {
// camera[0,1,2] are the angle-axis rotation.
T p[3];
ceres::AngleAxisRotatePoint(camera, point, p);
// camera[3,4,5] are the translation.
p[0] += camera[3];
p[1] += camera[4];
p[2] += camera[5];
// Compute the center of distortion.
T xp = p[0] / p[2];
T yp = p[1] / p[2];
// Compute final projected point position.
T predicted_x = fx * xp;
T predicted_y = fy * yp;
// The error is the difference between the predicted and observed position.
residuals[0] = predicted_x - observed_x;
residuals[1] = predicted_y - observed_y;
return true;
}
// Factory to hide the construction of the CostFunction object from
// the client code.
static ceres::CostFunction* Create(const double observed_x,
const double observed_y,
const double fx,
const double fy) {
return (new ceres::AutoDiffCostFunction<SnavelyReprojectionError, 2, 6, 3>(
new SnavelyReprojectionError(observed_x, observed_y, fx, fy)));
}
double observed_x;
double observed_y;
double fx;
double fy;
};
}
#endif /* CORELIB_SRC_OPTIMIZER_CERES_BUNDLE_SNAVELY_REPROJECTION_ERROR_H_ */

46
corelib/src/optimizer/ceres/pose_graph_2d/README.md Normal file
View File

@@ -0,0 +1,46 @@
Pose Graph 2D
----------------
The Simultaneous Localization and Mapping (SLAM) problem consists of building a
map of an unknown environment while simultaneously localizing against this
map. The main difficulty of this problem stems from not having any additional
external aiding information such as GPS. SLAM has been considered one of the
fundamental challenges of robotics. A pose graph optimization problem is one
example of a SLAM problem.
This package defines the necessary Ceres cost functions needed to model the
2-dimensional pose graph optimization problem as well as a binary to build and
solve the problem. The cost functions are shown for instruction purposes and can
be speed up by using analytical derivatives which take longer to implement.
Running
-----------
This package includes an executable `pose_graph_2d` that will read a problem
definition file. This executable can work with any 2D problem definition that
uses the g2o format. It would be relatively straightforward to implement a new
reader for a different format such as TORO or others. `pose_graph_2d` will print
the Ceres solver full summary and then output to disk the original and optimized
poses (`poses_original.txt` and `poses_optimized.txt`, respectively) of the
robot in the following format:
```
pose_id x y yaw_radians
pose_id x y yaw_radians
pose_id x y yaw_radians
...
```
where `pose_id` is the corresponding integer ID from the file definition. Note,
the file will be sorted in ascending order for the `pose_id`.
The executable `pose_graph_2d` has one flag `--input` which is the path to the
problem definition. To run the executable,
```
/path/to/bin/pose_graph_2d --input /path/to/dataset/dataset.g2o
```
A python script is provided to visualize the resulting output files.
```
/path/to/repo/examples/slam/pose_graph_2d/plot_results.py --optimized_poses ./poses_optimized.txt --initial_poses ./poses_original.txt
```

63
corelib/src/optimizer/ceres/pose_graph_2d/angle_local_parameterization.h Normal file
View File

@@ -0,0 +1,63 @@
// Ceres Solver - A fast non-linear least squares minimizer
// Copyright 2016 Google Inc. All rights reserved.
// http://ceres-solver.org/
//
// Redistribution and use in source and binary forms, with or without
// modification, are permitted provided that the following conditions are met:
//
// * Redistributions of source code must retain the above copyright notice,
// this list of conditions and the following disclaimer.
// * Redistributions in binary form must reproduce the above copyright notice,
// this list of conditions and the following disclaimer in the documentation
// and/or other materials provided with the distribution.
// * Neither the name of Google Inc. nor the names of its contributors may be
// used to endorse or promote products derived from this software without
// specific prior written permission.
//
// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
// POSSIBILITY OF SUCH DAMAGE.
//
// Author: vitus@google.com (Michael Vitus)
#ifndef CERES_EXAMPLES_POSE_GRAPH_2D_ANGLE_LOCAL_PARAMETERIZATION_H_
#define CERES_EXAMPLES_POSE_GRAPH_2D_ANGLE_LOCAL_PARAMETERIZATION_H_
#include "ceres/local_parameterization.h"
#include "normalize_angle.h"
namespace ceres {
namespace examples {
// Defines a local parameterization for updating the angle to be constrained in
// [-pi to pi).
class AngleLocalParameterization {
public:
template <typename T>
bool operator()(const T* theta_radians, const T* delta_theta_radians,
T* theta_radians_plus_delta) const {
*theta_radians_plus_delta =
NormalizeAngle(*theta_radians + *delta_theta_radians);
return true;
}
static ceres::LocalParameterization* Create() {
return (new ceres::AutoDiffLocalParameterization<AngleLocalParameterization,
1, 1>);
}
};
} // namespace examples
} // namespace ceres
#endif // CERES_EXAMPLES_POSE_GRAPH_2D_ANGLE_LOCAL_PARAMETERIZATION_H_

67
corelib/src/optimizer/ceres/pose_graph_2d/normalize_angle.h Normal file
View File

@@ -0,0 +1,67 @@
// Ceres Solver - A fast non-linear least squares minimizer
// Copyright 2016 Google Inc. All rights reserved.
// http://ceres-solver.org/
//
// Redistribution and use in source and binary forms, with or without
// modification, are permitted provided that the following conditions are met:
//
// * Redistributions of source code must retain the above copyright notice,
// this list of conditions and the following disclaimer.
// * Redistributions in binary form must reproduce the above copyright notice,
// this list of conditions and the following disclaimer in the documentation
// and/or other materials provided with the distribution.
// * Neither the name of Google Inc. nor the names of its contributors may be
// used to endorse or promote products derived from this software without
// specific prior written permission.
//
// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
// POSSIBILITY OF SUCH DAMAGE.
//
// Author: vitus@google.com (Michael Vitus)
#ifndef CERES_EXAMPLES_POSE_GRAPH_2D_NORMALIZE_ANGLE_H_
#define CERES_EXAMPLES_POSE_GRAPH_2D_NORMALIZE_ANGLE_H_
#include <cmath>
#include "ceres/ceres.h"
namespace ceres {
#if not(CERES_VERSION_MAJOR > 1 || (CERES_VERSION_MAJOR == 1 && CERES_VERSION_MINOR >= 12))
inline double floor (double x) { return std::floor(x); }
// The floor function should be used with extreme care as this operation will
// result in a zero derivative which provides no information to the solver.
//
// floor(a + h) ~= floor(a) + 0
template <typename T, int N> inline
Jet<T, N> floor(const Jet<T, N>& f) {
return Jet<T, N>(floor(f.a));
}
#endif
namespace examples {
// Normalizes the angle in radians between [-pi and pi).
template <typename T>
inline T NormalizeAngle(const T& angle_radians) {
// Use ceres::floor because it is specialized for double and Jet types.
T two_pi(2.0 * M_PI);
return angle_radians -
two_pi * ceres::floor((angle_radians + T(M_PI)) / two_pi);
}
} // namespace examples
} // namespace ceres
#endif // CERES_EXAMPLES_POSE_GRAPH_2D_NORMALIZE_ANGLE_H_

112
corelib/src/optimizer/ceres/pose_graph_2d/pose_graph_2d_error_term.h Normal file
View File

@@ -0,0 +1,112 @@
// Ceres Solver - A fast non-linear least squares minimizer
// Copyright 2016 Google Inc. All rights reserved.
// http://ceres-solver.org/
//
// Redistribution and use in source and binary forms, with or without
// modification, are permitted provided that the following conditions are met:
//
// * Redistributions of source code must retain the above copyright notice,
// this list of conditions and the following disclaimer.
// * Redistributions in binary form must reproduce the above copyright notice,
// this list of conditions and the following disclaimer in the documentation
// and/or other materials provided with the distribution.
// * Neither the name of Google Inc. nor the names of its contributors may be
// used to endorse or promote products derived from this software without
// specific prior written permission.
//
// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
// POSSIBILITY OF SUCH DAMAGE.
//
// Author: vitus@google.com (Michael Vitus)
//
// Cost function for a 2D pose graph formulation.
#ifndef CERES_EXAMPLES_POSE_GRAPH_2D_POSE_GRAPH_2D_ERROR_TERM_H_
#define CERES_EXAMPLES_POSE_GRAPH_2D_POSE_GRAPH_2D_ERROR_TERM_H_
#include "Eigen/Core"
namespace ceres {
namespace examples {
template <typename T>
Eigen::Matrix<T, 2, 2> RotationMatrix2D(T yaw_radians) {
const T cos_yaw = ceres::cos(yaw_radians);
const T sin_yaw = ceres::sin(yaw_radians);
Eigen::Matrix<T, 2, 2> rotation;
rotation << cos_yaw, -sin_yaw, sin_yaw, cos_yaw;
return rotation;
}
// Computes the error term for two poses that have a relative pose measurement
// between them. Let the hat variables be the measurement.
//
// residual = information^{1/2} * [ r_a^T * (p_b - p_a) - \hat{p_ab} ]
// [ Normalize(yaw_b - yaw_a - \hat{yaw_ab}) ]
//
// where r_a is the rotation matrix that rotates a vector represented in frame A
// into the global frame, and Normalize(*) ensures the angles are in the range
// [-pi, pi).
class PoseGraph2dErrorTerm {
public:
PoseGraph2dErrorTerm(double x_ab, double y_ab, double yaw_ab_radians,
const Eigen::Matrix3d& sqrt_information)
: p_ab_(x_ab, y_ab),
yaw_ab_radians_(yaw_ab_radians),
sqrt_information_(sqrt_information) {}
template <typename T>
bool operator()(const T* const x_a, const T* const y_a, const T* const yaw_a,
const T* const x_b, const T* const y_b, const T* const yaw_b,
T* residuals_ptr) const {
const Eigen::Matrix<T, 2, 1> p_a(*x_a, *y_a);
const Eigen::Matrix<T, 2, 1> p_b(*x_b, *y_b);
Eigen::Map<Eigen::Matrix<T, 3, 1> > residuals_map(residuals_ptr);
residuals_map.template head<2>() =
RotationMatrix2D(*yaw_a).transpose() * (p_b - p_a) -
p_ab_.cast<T>();
residuals_map(2) = ceres::examples::NormalizeAngle(
(*yaw_b - *yaw_a) - static_cast<T>(yaw_ab_radians_));
// Scale the residuals by the square root information matrix to account for
// the measurement uncertainty.
residuals_map = sqrt_information_.template cast<T>() * residuals_map;
return true;
}
static ceres::CostFunction* Create(double x_ab, double y_ab,
double yaw_ab_radians,
const Eigen::Matrix3d& sqrt_information) {
return (new ceres::AutoDiffCostFunction<PoseGraph2dErrorTerm, 3, 1, 1, 1, 1,
1, 1>(new PoseGraph2dErrorTerm(
x_ab, y_ab, yaw_ab_radians, sqrt_information)));
}
EIGEN_MAKE_ALIGNED_OPERATOR_NEW
private:
// The position of B relative to A in the A frame.
const Eigen::Vector2d p_ab_;
// The orientation of frame B relative to frame A.
const double yaw_ab_radians_;
// The inverse square root of the measurement covariance matrix.
const Eigen::Matrix3d sqrt_information_;
};
} // namespace examples
} // namespace ceres
#endif // CERES_EXAMPLES_POSE_GRAPH_2D_POSE_GRAPH_2D_ERROR_TERM_H_

105
corelib/src/optimizer/ceres/pose_graph_2d/types.h Normal file
View File

@@ -0,0 +1,105 @@
// Ceres Solver - A fast non-linear least squares minimizer
// Copyright 2016 Google Inc. All rights reserved.
// http://ceres-solver.org/
//
// Redistribution and use in source and binary forms, with or without
// modification, are permitted provided that the following conditions are met:
//
// * Redistributions of source code must retain the above copyright notice,
// this list of conditions and the following disclaimer.
// * Redistributions in binary form must reproduce the above copyright notice,
// this list of conditions and the following disclaimer in the documentation
// and/or other materials provided with the distribution.
// * Neither the name of Google Inc. nor the names of its contributors may be
// used to endorse or promote products derived from this software without
// specific prior written permission.
//
// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
// POSSIBILITY OF SUCH DAMAGE.
//
// Author: vitus@google.com (Michael Vitus)
//
// Defines the types used in the 2D pose graph SLAM formulation. Each vertex of
// the graph has a unique integer ID with a position and orientation. There are
// delta transformation constraints between two vertices.
#ifndef CERES_EXAMPLES_POSE_GRAPH_2D_TYPES_H_
#define CERES_EXAMPLES_POSE_GRAPH_2D_TYPES_H_
#include <fstream>
#include "Eigen/Core"
#include "normalize_angle.h"
namespace ceres {
namespace examples {
// The state for each vertex in the pose graph.
struct Pose2d {
double x;
double y;
double yaw_radians;
// The name of the data type in the g2o file format.
static std::string name() {
return "VERTEX_SE2";
}
};
inline std::istream& operator>>(std::istream& input, Pose2d& pose) {
input >> pose.x >> pose.y >> pose.yaw_radians;
// Normalize the angle between -pi to pi.
pose.yaw_radians = NormalizeAngle(pose.yaw_radians);
return input;
}
// The constraint between two vertices in the pose graph. The constraint is the
// transformation from vertex id_begin to vertex id_end.
struct Constraint2d {
int id_begin;
int id_end;
double x;
double y;
double yaw_radians;
// The inverse of the covariance matrix for the measurement. The order of the
// entries are x, y, and yaw.
Eigen::Matrix3d information;
// The name of the data type in the g2o file format.
static std::string name() {
return "EDGE_SE2";
}
};
inline std::istream& operator>>(std::istream& input, Constraint2d& constraint) {
input >> constraint.id_begin >> constraint.id_end >> constraint.x >>
constraint.y >> constraint.yaw_radians >>
constraint.information(0, 0) >> constraint.information(0, 1) >>
constraint.information(0, 2) >> constraint.information(1, 1) >>
constraint.information(1, 2) >> constraint.information(2, 2);
// Set the lower triangular part of the information matrix.
constraint.information(1, 0) = constraint.information(0, 1);
constraint.information(2, 0) = constraint.information(0, 2);
constraint.information(2, 1) = constraint.information(1, 2);
// Normalize the angle between -pi to pi.
constraint.yaw_radians = NormalizeAngle(constraint.yaw_radians);
return input;
}
} // namespace examples
} // namespace ceres
#endif // CERES_EXAMPLES_POSE_GRAPH_2D_TYPES_H_

54
corelib/src/optimizer/ceres/pose_graph_3d/README.md Normal file
View File

@@ -0,0 +1,54 @@
Pose Graph 3D
----------------
The Simultaneous Localization and Mapping (SLAM) problem consists of building a
map of an unknown environment while simultaneously localizing against this
map. The main difficulty of this problem stems from not having any additional
external aiding information such as GPS. SLAM has been considered one of the
fundamental challenges of robotics. A pose graph optimization problem is one
example of a SLAM problem.
The example also illustrates how to use Eigen's geometry module with Ceres'
automatic differentiation functionality. To represent the orientation, we will
use Eigen's quaternion which uses the Hamiltonian convention but has different
element ordering as compared with Ceres's rotation representation. Specifically
they differ by whether the scalar component q_w is first or last; the element
order for Ceres's quaternion is [q_w, q_x, q_y, q_z] where as Eigen's quaternion
is [q_x, q_y, q_z, q_w].
This package defines the necessary Ceres cost functions needed to model the
3-dimensional pose graph optimization problem as well as a binary to build and
solve the problem. The cost functions are shown for instruction purposes and can
be speed up by using analytical derivatives which take longer to implement.
Running
-----------
This package includes an executable `pose_graph_3d` that will read a problem
definition file. This executable can work with any 3D problem definition that
uses the g2o format with quaternions used for the orientation representation. It
would be relatively straightforward to implement a new reader for a different
format such as TORO or others. `pose_graph_3d` will print the Ceres solver full
summary and then output to disk the original and optimized poses
(`poses_original.txt` and `poses_optimized.txt`, respectively) of the robot in
the following format:
```
pose_id x y z q_x q_y q_z q_w
pose_id x y z q_x q_y q_z q_w
pose_id x y z q_x q_y q_z q_w
...
```
where `pose_id` is the corresponding integer ID from the file definition. Note,
the file will be sorted in ascending order for the `pose_id`.
The executable `pose_graph_3d` has one flag `--input` which is the path to the
problem definition. To run the executable,
```
/path/to/bin/pose_graph_3d --input /path/to/dataset/dataset.g2o
```
A script is provided to visualize the resulting output files. There is also an
option to enable equal axes using ```--axes_equal```.
```
/path/to/repo/examples/slam/pose_graph_3d/plot_results.py --optimized_poses ./poses_optimized.txt --initial_poses ./poses_original.txt
```

90
corelib/src/optimizer/ceres/pose_graph_3d/eigen_quaternion_parameterization.h Normal file
View File

@@ -0,0 +1,90 @@
// Ceres Solver - A fast non-linear least squares minimizer
// Copyright 2016 Google Inc. All rights reserved.
// http://ceres-solver.org/
//
// Redistribution and use in source and binary forms, with or without
// modification, are permitted provided that the following conditions are met:
//
// * Redistributions of source code must retain the above copyright notice,
// this list of conditions and the following disclaimer.
// * Redistributions in binary form must reproduce the above copyright notice,
// this list of conditions and the following disclaimer in the documentation
// and/or other materials provided with the distribution.
// * Neither the name of Google Inc. nor the names of its contributors may be
// used to endorse or promote products derived from this software without
// specific prior written permission.
//
// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
// POSSIBILITY OF SUCH DAMAGE.
//
// Author: sameeragarwal@google.com (Sameer Agarwal)
#ifndef CERES_EXAMPLES_POSE_GRAPH_3D_EIGEN_QUATERNION_PARAMETERIZATION_H_
#define CERES_EXAMPLES_POSE_GRAPH_3D_EIGEN_QUATERNION_PARAMETERIZATION_H_
#include "ceres/local_parameterization.h"
namespace ceres {
// Implements the quaternion local parameterization for Eigen's representation
// of the quaternion. Eigen uses a different internal memory layout for the
// elements of the quaternion than what is commonly used. Specifically, Eigen
// stores the elements in memory as [x, y, z, w] where the real part is last
// whereas it is typically stored first. Note, when creating an Eigen quaternion
// through the constructor the elements are accepted in w, x, y, z order. Since
// Ceres operates on parameter blocks which are raw double pointers this
// difference is important and requires a different parameterization.
//
// Plus(x, delta) = [sin(|delta|) delta / |delta|, cos(|delta|)] * x
// with * being the quaternion multiplication operator.
class EigenQuaternionParameterization : public ceres::LocalParameterization {
public:
virtual ~EigenQuaternionParameterization() {}
virtual bool Plus(const double* x_ptr,
const double* delta,
double* x_plus_delta_ptr) const
{
Eigen::Map<Eigen::Quaterniond> x_plus_delta(x_plus_delta_ptr);
Eigen::Map<const Eigen::Quaterniond> x(x_ptr);
const double norm_delta =
sqrt(delta[0] * delta[0] + delta[1] * delta[1] + delta[2] * delta[2]);
if (norm_delta > 0.0) {
const double sin_delta_by_delta = sin(norm_delta) / norm_delta;
// Note, in the constructor w is first.
Eigen::Quaterniond delta_q(cos(norm_delta),
sin_delta_by_delta * delta[0],
sin_delta_by_delta * delta[1],
sin_delta_by_delta * delta[2]);
x_plus_delta = delta_q * x;
} else {
x_plus_delta = x;
}
return true;
}
virtual bool ComputeJacobian(const double* x,
double* jacobian) const{
jacobian[0] = x[3]; jacobian[1] = x[2]; jacobian[2] = -x[1]; // NOLINT
jacobian[3] = -x[2]; jacobian[4] = x[3]; jacobian[5] = x[0]; // NOLINT
jacobian[6] = x[1]; jacobian[7] = -x[0]; jacobian[8] = x[3]; // NOLINT
jacobian[9] = -x[0]; jacobian[10] = -x[1]; jacobian[11] = -x[2]; // NOLINT
return true;
}
virtual int GlobalSize() const { return 4; }
virtual int LocalSize() const { return 3; }
};
} // namespace ceres
#endif // CERES_EXAMPLES_POSE_GRAPH_3D_EIGEN_QUATERNION_PARAMETERIZATION_H_

131
corelib/src/optimizer/ceres/pose_graph_3d/pose_graph_3d_error_term.h Normal file
View File

@@ -0,0 +1,131 @@
// Ceres Solver - A fast non-linear least squares minimizer
// Copyright 2016 Google Inc. All rights reserved.
// http://ceres-solver.org/
//
// Redistribution and use in source and binary forms, with or without
// modification, are permitted provided that the following conditions are met:
//
// * Redistributions of source code must retain the above copyright notice,
// this list of conditions and the following disclaimer.
// * Redistributions in binary form must reproduce the above copyright notice,
// this list of conditions and the following disclaimer in the documentation
// and/or other materials provided with the distribution.
// * Neither the name of Google Inc. nor the names of its contributors may be
// used to endorse or promote products derived from this software without
// specific prior written permission.
//
// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
// POSSIBILITY OF SUCH DAMAGE.
//
// Author: vitus@google.com (Michael Vitus)
#ifndef EXAMPLES_CERES_POSE_GRAPH_3D_ERROR_TERM_H_
#define EXAMPLES_CERES_POSE_GRAPH_3D_ERROR_TERM_H_
#include "Eigen/Core"
#include "ceres/autodiff_cost_function.h"
#include "types.h"
namespace ceres {
namespace examples {
// Computes the error term for two poses that have a relative pose measurement
// between them. Let the hat variables be the measurement. We have two poses x_a
// and x_b. Through sensor measurements we can measure the transformation of
// frame B w.r.t frame A denoted as t_ab_hat. We can compute an error metric
// between the current estimate of the poses and the measurement.
//
// In this formulation, we have chosen to represent the rigid transformation as
// a Hamiltonian quaternion, q, and position, p. The quaternion ordering is
// [x, y, z, w].
// The estimated measurement is:
// t_ab = [ p_ab ] = [ R(q_a)^T * (p_b - p_a) ]
// [ q_ab ] [ q_a^{-1] * q_b ]
//
// where ^{-1} denotes the inverse and R(q) is the rotation matrix for the
// quaternion. Now we can compute an error metric between the estimated and
// measurement transformation. For the orientation error, we will use the
// standard multiplicative error resulting in:
//
// error = [ p_ab - \hat{p}_ab ]
// [ 2.0 * Vec(q_ab * \hat{q}_ab^{-1}) ]
//
// where Vec(*) returns the vector (imaginary) part of the quaternion. Since
// the measurement has an uncertainty associated with how accurate it is, we
// will weight the errors by the square root of the measurement information
// matrix:
//
// residuals = I^{1/2) * error
// where I is the information matrix which is the inverse of the covariance.
class PoseGraph3dErrorTerm {
public:
PoseGraph3dErrorTerm(const Pose3d& t_ab_measured,
const Eigen::Matrix<double, 6, 6>& sqrt_information)
: t_ab_measured_(t_ab_measured), sqrt_information_(sqrt_information) {}
template <typename T>
bool operator()(const T* const p_a_ptr, const T* const q_a_ptr,
const T* const p_b_ptr, const T* const q_b_ptr,
T* residuals_ptr) const {
Eigen::Map<const Eigen::Matrix<T, 3, 1> > p_a(p_a_ptr);
Eigen::Map<const Eigen::Quaternion<T> > q_a(q_a_ptr);
Eigen::Map<const Eigen::Matrix<T, 3, 1> > p_b(p_b_ptr);
Eigen::Map<const Eigen::Quaternion<T> > q_b(q_b_ptr);
// Compute the relative transformation between the two frames.
Eigen::Quaternion<T> q_a_inverse = q_a.conjugate();
Eigen::Quaternion<T> q_ab_estimated = q_a_inverse * q_b;
// Represent the displacement between the two frames in the A frame.
Eigen::Matrix<T, 3, 1> p_ab_estimated = q_a_inverse * (p_b - p_a);
// Compute the error between the two orientation estimates.
Eigen::Quaternion<T> delta_q =
t_ab_measured_.q.template cast<T>() * q_ab_estimated.conjugate();
// Compute the residuals.
// [ position ] [ delta_p ]
// [ orientation (3x1)] = [ 2 * delta_q(0:2) ]
Eigen::Map<Eigen::Matrix<T, 6, 1> > residuals(residuals_ptr);
residuals.template block<3, 1>(0, 0) =
p_ab_estimated - t_ab_measured_.p.template cast<T>();
residuals.template block<3, 1>(3, 0) = T(2.0) * delta_q.vec();
// Scale the residuals by the measurement uncertainty.
residuals.applyOnTheLeft(sqrt_information_.template cast<T>());
return true;
}
static ceres::CostFunction* Create(
const Pose3d& t_ab_measured,
const Eigen::Matrix<double, 6, 6>& sqrt_information) {
return new ceres::AutoDiffCostFunction<PoseGraph3dErrorTerm, 6, 3, 4, 3, 4>(
new PoseGraph3dErrorTerm(t_ab_measured, sqrt_information));
}
EIGEN_MAKE_ALIGNED_OPERATOR_NEW
private:
// The measurement for the position of B relative to A in the A frame.
const Pose3d t_ab_measured_;
// The square root of the measurement information matrix.
const Eigen::Matrix<double, 6, 6> sqrt_information_;
};
} // namespace examples
} // namespace ceres
#endif // EXAMPLES_CERES_POSE_GRAPH_3D_ERROR_TERM_H_

114
corelib/src/optimizer/ceres/pose_graph_3d/types.h Normal file
View File

@@ -0,0 +1,114 @@
// Ceres Solver - A fast non-linear least squares minimizer
// Copyright 2016 Google Inc. All rights reserved.
// http://ceres-solver.org/
//
// Redistribution and use in source and binary forms, with or without
// modification, are permitted provided that the following conditions are met:
//
// * Redistributions of source code must retain the above copyright notice,
// this list of conditions and the following disclaimer.
// * Redistributions in binary form must reproduce the above copyright notice,
// this list of conditions and the following disclaimer in the documentation
// and/or other materials provided with the distribution.
// * Neither the name of Google Inc. nor the names of its contributors may be
// used to endorse or promote products derived from this software without
// specific prior written permission.
//
// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
// POSSIBILITY OF SUCH DAMAGE.
//
// Author: vitus@google.com (Michael Vitus)
#ifndef EXAMPLES_CERES_TYPES_H_
#define EXAMPLES_CERES_TYPES_H_
#include <istream>
#include <map>
#include <string>
#include <vector>
#include "Eigen/Core"
#include "Eigen/Geometry"
namespace ceres {
namespace examples {
struct Pose3d {
Eigen::Vector3d p;
Eigen::Quaterniond q;
// The name of the data type in the g2o file format.
static std::string name() {
return "VERTEX_SE3:QUAT";
}
EIGEN_MAKE_ALIGNED_OPERATOR_NEW
};
inline std::istream& operator>>(std::istream& input, Pose3d& pose) {
input >> pose.p.x() >> pose.p.y() >> pose.p.z() >> pose.q.x() >>
pose.q.y() >> pose.q.z() >> pose.q.w();
// Normalize the quaternion to account for precision loss due to
// serialization.
pose.q.normalize();
return input;
}
typedef std::map<int, Pose3d, std::less<int>,
Eigen::aligned_allocator<std::pair<const int, Pose3d> > >
MapOfPoses;
// The constraint between two vertices in the pose graph. The constraint is the
// transformation from vertex id_begin to vertex id_end.
struct Constraint3d {
int id_begin;
int id_end;
// The transformation that represents the pose of the end frame E w.r.t. the
// begin frame B. In other words, it transforms a vector in the E frame to
// the B frame.
Pose3d t_be;
// The inverse of the covariance matrix for the measurement. The order of the
// entries are x, y, z, delta orientation.
Eigen::Matrix<double, 6, 6> information;
// The name of the data type in the g2o file format.
static std::string name() {
return "EDGE_SE3:QUAT";
}
EIGEN_MAKE_ALIGNED_OPERATOR_NEW
};
inline std::istream& operator>>(std::istream& input, Constraint3d& constraint) {
Pose3d& t_be = constraint.t_be;
input >> constraint.id_begin >> constraint.id_end >> t_be;
for (int i = 0; i < 6 && input.good(); ++i) {
for (int j = i; j < 6 && input.good(); ++j) {
input >> constraint.information(i, j);
if (i != j) {
constraint.information(j, i) = constraint.information(i, j);
}
}
}
return input;
}
typedef std::vector<Constraint3d, Eigen::aligned_allocator<Constraint3d> >
VectorOfConstraints;
} // namespace examples
} // namespace ceres
#endif // EXAMPLES_CERES_TYPES_H_