Skip to content

Commit

Permalink
fixxing bugs
Browse files Browse the repository at this point in the history
  • Loading branch information
gaoxiang12 committed Nov 16, 2016
1 parent 3c14aeb commit a76537b
Show file tree
Hide file tree
Showing 30 changed files with 4,373 additions and 80 deletions.
129 changes: 77 additions & 52 deletions ch10/g2o_custombundle/g2o_bal_class.h
View file
Original file line number Diff line number Diff line change
@@ -1,7 +1,7 @@
#include <Eigen/Core>
#include "g2o/core/base_vertex.h"
#include "g2o/core/base_binary_edge.h"

#include <Eigen/Core>
#include "ceres/autodiff.h"

#include "tools/rotation.h"
Expand All @@ -11,16 +11,23 @@ class VertexCameraBAL : public g2o::BaseVertex<9,Eigen::VectorXd>
{
public:
EIGEN_MAKE_ALIGNED_OPERATOR_NEW;
VertexCameraBAL(){}
VertexCameraBAL() {}

virtual bool read(std::istream& /*is*/){return false;}
virtual bool read ( std::istream& /*is*/ )
{
return false;
}

virtual bool write (std::ostream& /*os*/)const {return false;}
virtual bool write ( std::ostream& /*os*/ ) const
{
return false;
}

virtual void setToOriginImpl(){}
virtual void setToOriginImpl() {}

virtual void oplusImpl(const double* update){
Eigen::VectorXd::ConstMapType v(update, VertexCameraBAL::Dimension);
virtual void oplusImpl ( const double* update )
{
Eigen::VectorXd::ConstMapType v ( update, VertexCameraBAL::Dimension );
_estimate += v;
}

Expand All @@ -31,76 +38,94 @@ class VertexPointBAL : public g2o::BaseVertex<3, Eigen::Vector3d>
{
public:
EIGEN_MAKE_ALIGNED_OPERATOR_NEW;
VertexPointBAL(){}
VertexPointBAL() {}

virtual bool read(std::istream& /*is*/){return false;}
virtual bool read ( std::istream& /*is*/ )
{
return false;
}

virtual bool write(std::ostream& /*os*/)const {return false;}
virtual bool write ( std::ostream& /*os*/ ) const
{
return false;
}

virtual void setToOriginImpl(){}
virtual void setToOriginImpl() {}

virtual void oplusImpl(const double* update){
Eigen::Vector3d::ConstMapType v(update);
virtual void oplusImpl ( const double* update )
{
Eigen::Vector3d::ConstMapType v ( update );
_estimate += v;
}
};

class EdgeObservationBAL : public g2o::BaseBinaryEdge<2, Eigen::Vector2d,VertexCameraBAL, VertexPointBAL>{
class EdgeObservationBAL : public g2o::BaseBinaryEdge<2, Eigen::Vector2d,VertexCameraBAL, VertexPointBAL>
{
public:
EIGEN_MAKE_ALIGNED_OPERATOR_NEW;
EdgeObservationBAL(){}

virtual bool read(std::istream& /*is*/){ return false;}
EdgeObservationBAL() {}

virtual bool read ( std::istream& /*is*/ )
{
return false;
}

virtual bool write(std::ostream& /*os*/)const { return false;}
virtual bool write ( std::ostream& /*os*/ ) const
{
return false;
}

virtual void computeError() override // The virtual function comes from the Edge base class. Must define if you use edge.
{
const VertexCameraBAL* cam = static_cast<const VertexCameraBAL*>(vertex(0));
const VertexPointBAL* point = static_cast<const VertexPointBAL*>(vertex(1));
const VertexCameraBAL* cam = static_cast<const VertexCameraBAL*> ( vertex ( 0 ) );
const VertexPointBAL* point = static_cast<const VertexPointBAL*> ( vertex ( 1 ) );

( *this ) ( cam->estimate().data(), point->estimate().data(), _error.data() );

(*this)(cam->estimate().data(), point->estimate().data(), _error.data());

}

template<typename T>
bool operator()(const T* camera, const T* point, T* residuals)const
bool operator() ( const T* camera, const T* point, T* residuals ) const
{
T predictions[2];
CamProjectionWithDistortion(camera, point, predictions);
residuals[0] = predictions[0] - T(measurement()(0));
residuals[1] = predictions[1] - T(measurement()(1));
CamProjectionWithDistortion ( camera, point, predictions );
residuals[0] = predictions[0] - T ( measurement() ( 0 ) );
residuals[1] = predictions[1] - T ( measurement() ( 1 ) );

return true;
}


virtual void linearizeOplus() override
{
// use numeric Jacobians
// BaseBinaryEdge<2, Vector2d, VertexCameraBAL, VertexPointBAL>::linearizeOplus();
// return;
// using autodiff from ceres. Otherwise, the system will use g2o numerical differences for Jacobians

const VertexCameraBAL* cam = static_cast<const VertexCameraBAL*>(vertex(0));
const VertexPointBAL* point = static_cast<const VertexPointBAL*>(vertex(1));
typedef ceres::internal::AutoDiff<EdgeObservationBAL, double, VertexCameraBAL::Dimension, VertexPointBAL::Dimension> BalAutoDiff;

Eigen::Matrix<double, Dimension, VertexCameraBAL::Dimension, Eigen::RowMajor> dError_dCamera;
Eigen::Matrix<double, Dimension, VertexPointBAL::Dimension, Eigen::RowMajor> dError_dPoint;
double *parameters[] = { const_cast<double*>(cam->estimate().data()), const_cast<double*>(point->estimate().data()) };
double *jacobians[] = { dError_dCamera.data(), dError_dPoint.data() };
double value[Dimension];
bool diffState = BalAutoDiff::Differentiate(*this, parameters, Dimension, value, jacobians);

// copy over the Jacobians (convert row-major -> column-major)
if (diffState) {
_jacobianOplusXi = dError_dCamera;
_jacobianOplusXj = dError_dPoint;
} else {
assert(0 && "Error while differentiating");
_jacobianOplusXi.setZero();
_jacobianOplusXi.setZero();
}
// use numeric Jacobians
// BaseBinaryEdge<2, Vector2d, VertexCameraBAL, VertexPointBAL>::linearizeOplus();
// return;

// using autodiff from ceres. Otherwise, the system will use g2o numerical diff for Jacobians

const VertexCameraBAL* cam = static_cast<const VertexCameraBAL*> ( vertex ( 0 ) );
const VertexPointBAL* point = static_cast<const VertexPointBAL*> ( vertex ( 1 ) );
typedef ceres::internal::AutoDiff<EdgeObservationBAL, double, VertexCameraBAL::Dimension, VertexPointBAL::Dimension> BalAutoDiff;

Eigen::Matrix<double, Dimension, VertexCameraBAL::Dimension, Eigen::RowMajor> dError_dCamera;
Eigen::Matrix<double, Dimension, VertexPointBAL::Dimension, Eigen::RowMajor> dError_dPoint;
double *parameters[] = { const_cast<double*> ( cam->estimate().data() ), const_cast<double*> ( point->estimate().data() ) };
double *jacobians[] = { dError_dCamera.data(), dError_dPoint.data() };
double value[Dimension];
bool diffState = BalAutoDiff::Differentiate ( *this, parameters, Dimension, value, jacobians );

// copy over the Jacobians (convert row-major -> column-major)
if ( diffState )
{
_jacobianOplusXi = dError_dCamera;
_jacobianOplusXj = dError_dPoint;
}
else
{
assert ( 0 && "Error while differentiating" );
_jacobianOplusXi.setZero();
_jacobianOplusXi.setZero();
}
}
};
};
1 change: 1 addition & 0 deletions ch2/CMakeLists.txt
View file
Original file line number Diff line number Diff line change
Expand Up @@ -17,4 +17,5 @@ add_library( hello libHelloSLAM.cpp )
add_library( hello_shared SHARED libHelloSLAM.cpp )

add_executable( useHello useHello.cpp )
# 将库文件链接到可执行程序上
target_link_libraries( useHello hello_shared )
3 changes: 2 additions & 1 deletion ch2/useHello.cpp
View file
Original file line number Diff line number Diff line change
@@ -1,7 +1,8 @@
#include "libHelloSLAM.h"

// 使用 libHelloSLAM.h 中的 printHello() 函数
int main( int argc, char** argv )
{
printHello();
return 0;
}
}
42 changes: 27 additions & 15 deletions ch3/useEigen/eigenMatrix.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -5,26 +5,35 @@ using namespace std;
#include <Eigen/Core>
// 稠密矩阵的代数运算（逆，特征值等）
#include <Eigen/Dense>

#define MATRIX_SIZE 50

/****************************
* 本程序演示了 Eigen 基本类型的使用
****************************/

int main( int argc, char** argv )
{
// Eigen 中所有向量和矩阵都是Eigen::Matrix，它是一个模板类。它的前三个参数为：数据类型，行，列
// 声明一个2*3的float矩阵
Eigen::Matrix<float, 2, 3> matrix_23;

// 同时，Eigen 通过 typedef 提供了许多内置类型，不过底层仍是Eigen::Matrix
// 例如 Vector3d 实质上是 Eigen::Matrix<double, 3, 1>
// 例如 Vector3d 实质上是 Eigen::Matrix<double, 3, 1>，即三维向量
Eigen::Vector3d v_3d;
// 这是一样的
Eigen::Matrix<float,3,1> vd_3d;
// 还有 Matrix3d 实质上是 Eigen::Matrix<double, 3, 3>

// Matrix3d 实质上是 Eigen::Matrix<double, 3, 3>
Eigen::Matrix3d matrix_33 = Eigen::Matrix3d::Zero(); //初始化为零
// 如果您不确定矩阵大小，可以使用动态大小的矩阵
// 如果不确定矩阵大小，可以使用动态大小的矩阵
Eigen::Matrix< double, Eigen::Dynamic, Eigen::Dynamic > matrix_dynamic;
// 更简单的
Eigen::MatrixXd matrix_x;
// 这种类型还有很多，我们不一一列举

// 下面是对Eigen阵的操作
// 输入数据
// 输入数据（初始化）
matrix_23 << 1, 2, 3, 4, 5, 6;
// 输出
cout << matrix_23 << endl;
Expand All @@ -35,12 +44,12 @@ int main( int argc, char** argv )
cout<<matrix_23(i,j)<<"\t";
cout<<endl;
}

// 矩阵和向量相乘（实际上仍是矩阵和矩阵）
v_3d << 3, 2, 1;
vd_3d<<4,5,6;
vd_3d << 4,5,6;
// 但是在Eigen里你不能混合两种不同类型的矩阵，像这样是错的
// Eigen::Matrix<double, 2, 1> result_wrong_type = matrix_23 * v_3d;

// 应该显式转换
Eigen::Matrix<double, 2, 1> result = matrix_23.cast<double>() * v_3d;
cout << result << endl;
Expand All @@ -54,15 +63,15 @@ int main( int argc, char** argv )

// 一些矩阵运算
// 四则运算就不演示了，直接用+-*/即可。
matrix_33 = Eigen::Matrix3d::Random();
matrix_33 = Eigen::Matrix3d::Random(); // 随机数矩阵
cout << matrix_33 << endl << endl;

cout << matrix_33.transpose() << endl; //转置
cout << matrix_33.sum() << endl; //各元素和
cout << matrix_33.trace() << endl; //迹
cout << 10*matrix_33 << endl; //数乘
cout << matrix_33.inverse() << endl; //逆
cout << matrix_33.determinant() << endl; //行列式
cout << matrix_33.transpose() << endl; // 转置
cout << matrix_33.sum() << endl; // 各元素和
cout << matrix_33.trace() << endl; // 迹
cout << 10*matrix_33 << endl; // 数乘
cout << matrix_33.inverse() << endl; // 逆
cout << matrix_33.determinant() << endl; // 行列式

// 特征值
Eigen::SelfAdjointEigenSolver<Eigen::Matrix3d> eigen_solver ( matrix_33 );
Expand All @@ -78,11 +87,14 @@ int main( int argc, char** argv )
matrix_NN = Eigen::MatrixXd::Random( MATRIX_SIZE, MATRIX_SIZE );
Eigen::Matrix< double, MATRIX_SIZE, 1> v_Nd;
v_Nd = Eigen::MatrixXd::Random( MATRIX_SIZE,1 );
clock_t time_stt = clock();

clock_t time_stt = clock(); // 计时
// 直接求逆
Eigen::Matrix<double,MATRIX_SIZE,1> x = matrix_NN.inverse()*v_Nd;
cout <<"time use in normal invers is " << 1000* (clock() - time_stt)/(double)CLOCKS_PER_SEC << "ms"<< endl;

// 通常用矩阵分解来求，例如QR分解，速度会快很多
time_stt = clock();
// 通常用矩阵分解来求，例如QR分解，速度会快很多
x = matrix_NN.colPivHouseholderQr().solve(v_Nd);
cout <<"time use in Qr compsition is " <<1000* (clock() - time_stt)/(double)CLOCKS_PER_SEC <<"ms" << endl;

Expand Down
22 changes: 13 additions & 9 deletions ch3/useGeometry/eigenGeometry.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -6,40 +6,44 @@ using namespace std;
// Eigen 几何模块
#include <Eigen/Geometry>

/****************************
* 本程序演示了 Eigen 几何模块的使用方法
****************************/

int main ( int argc, char** argv )
{
// Eigen/Geometry 模块提供了各种旋转和平移的表示
// 3D 旋转矩阵直接使用 Matrix3d 或 Matrix3f
Eigen::Matrix3d rotation_matrix = Eigen::Matrix3d::Identity();
// 旋转向量使用 AngleAxis, 它底层不直接是Matrix，但运算可以当作矩阵（因为重载了运算符）
Eigen::AngleAxisd rotation_vector ( M_PI/4, Eigen::Vector3d ( 0,0,1 ) ); //沿Z轴旋转90度
Eigen::AngleAxisd rotation_vector ( M_PI/4, Eigen::Vector3d ( 0,0,1 ) ); //沿 Z 轴旋转 45 度
cout .precision(3);
cout<<"rotation matrix =\n"<<rotation_vector.matrix() <<endl; //用matrix()转换成矩阵
// 也可以直接赋值
rotation_matrix = rotation_vector.toRotationMatrix();
// 用 AngleAxis 可以进行坐标变换
Eigen::Vector3d v ( 1,0,0 );
Eigen::Vector3d v_rotated = rotation_vector * v;
cout<<"(1,0,0) after rotation = \n"<<v_rotated<<endl;
cout<<"(1,0,0) after rotation = "<<v_rotated.transpose()<<endl;
// 或者用旋转矩阵
v_rotated = rotation_matrix * v;
cout<<"(1,0,0) after rotation = \n"<<v_rotated<<endl;
cout<<"(1,0,0) after rotation = "<<v_rotated.transpose()<<endl;

// 欧拉角: 可以将旋转矩阵直接转换成欧拉角
Eigen::Vector3d euler_angles = rotation_matrix.eulerAngles ( 0,1,2 ); //x-y-z顺序，即roll pitch yaw顺序
cout<<"roll pitch yaw = \n"<<euler_angles<<endl;
Eigen::Vector3d euler_angles = rotation_matrix.eulerAngles ( 2,1,0 ); // ZYX顺序，即roll pitch yaw顺序
cout<<"yaw pitch roll = "<<euler_angles.transpose()<<endl;

// 欧氏变换矩阵使用 Eigen::Isometry
Eigen::Isometry3d T=Eigen::Isometry3d::Identity(); // 虽然称为3d，实质上是4＊4的矩阵
T.rotate ( rotation_vector ); // 按照rotation_vector进行旋转
T.pretranslate ( Eigen::Vector3d ( 1,3,4 ) ); // 沿X轴平移1，相当于把平移向量设成(1,0,0)
T.pretranslate ( Eigen::Vector3d ( 1,3,4 ) ); // 把平移向量设成(1,3,4)
cout << "Transform matrix = \n" << T.matrix() <<endl;

// 用变换矩阵进行坐标变换
Eigen::Vector3d v_transformed = T*v; // 相当于R*v+t
cout<<"v tranformed = \n"<<v_transformed<<endl;
cout<<"v tranformed = "<<v_transformed.transpose()<<endl;

// 对于仿射和射影变换，使用 Eigen::Affine3d 和 Eigen::Projective3d 即可
// 对于仿射和射影变换，使用 Eigen::Affine3d 和 Eigen::Projective3d 即可，略

// 四元数
// 可以直接把AngleAxis赋值给四元数，反之亦然
Expand All @@ -50,7 +54,7 @@ int main ( int argc, char** argv )
cout<<"quaternion = \n"<<q.coeffs() <<endl;
// 使用四元数旋转一个向量，使用重载的乘法即可
v_rotated = q*v; // 注意数学上是qvq^{-1}
cout<<"(1,0,0) after rotation = \n"<<v_rotated<<endl;
cout<<"(1,0,0) after rotation = "<<v_rotated.transpose()<<endl;

return 0;
}
1 change: 0 additions & 1 deletion ch3/visualizeGeometry/CMakeLists.txt
View file
Original file line number Diff line number Diff line change
Expand Up @@ -6,7 +6,6 @@ set(CMAKE_CXX_FLAGS "-std=c++11")
# 添加Eigen头文件
include_directories( "/usr/include/eigen3" )


# 添加Pangolin依赖
find_package( Pangolin )
include_directories( ${Pangolin_INCLUDE_DIRS} )
Expand Down
3 changes: 1 addition & 2 deletions ch3/visualizeGeometry/visualizeGeometry.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -96,7 +96,6 @@ int main ( int argc, char** argv )
for (int i=0; i<3; i++)
for (int j=0; j<3; j++)
R.matrix(i,j) = m(j,i);
// R.matrix = R.matrix.transpose();
rotation_matrix = R;

TranslationVector t;
Expand All @@ -105,7 +104,7 @@ int main ( int argc, char** argv )
translation_vector = t;

TranslationVector euler;
euler.trans = R.matrix.eulerAngles(2,0,1);
euler.trans = R.matrix.eulerAngles(2,1,1);
euler_angles = euler;

QuaternionDraw quat;
Expand Down
Loading

0 comments on commit a76537b

Please sign in to comment.