//==============================================================================
void ShapeNode::setRelativeTransform(const Eigen::Isometry3d& transform)
{
if(transform.matrix() == FixedFrame::mAspectProperties.mRelativeTf.matrix())
return;
const Eigen::Isometry3d oldTransform = getRelativeTransform();
FixedFrame::setRelativeTransform(transform);
dirtyJacobian();
dirtyJacobianDeriv();
mRelativeTransformUpdatedSignal.raise(
this, oldTransform, getRelativeTransform());
}
int collideBoxBox(const Eigen::Vector3d& size0, const Eigen::Isometry3d& T0,
const Eigen::Vector3d& size1, const Eigen::Isometry3d& T1,
std::vector<Contact>* result)
{
dVector3 halfSize0;
dVector3 halfSize1;
convVector(0.5 * size0, halfSize0);
convVector(0.5 * size1, halfSize1);
dMatrix3 R0, R1;
convMatrix(T0, R0);
convMatrix(T1, R1);
dVector3 p0;
dVector3 p1;
convVector(T0.translation(), p0);
convVector(T1.translation(), p1);
return dBoxBox(p1, R1, halfSize1, p0, R0, halfSize0, *result);
}
const double CMiniVisionToolbox::getTransformationDelta( const Eigen::Isometry3d& p_matTransformationFrom, const Eigen::Isometry3d& p_matTransformationTo )
{
//ds compute pose change
const Eigen::Isometry3d matTransformChange( p_matTransformationTo*p_matTransformationFrom.inverse( ) );
//ds check point
const Eigen::Vector4d vecSamplePoint( 40.2, -1.25, 2.5, 1.0 );
double dNorm( vecSamplePoint.norm( ) );
const Eigen::Vector4d vecDifference( vecSamplePoint-matTransformChange*vecSamplePoint );
dNorm = ( dNorm + vecDifference.norm( ) )/2;
//ds return norm
return ( std::fabs( vecDifference(0) ) + std::fabs( vecDifference(1) ) + std::fabs( vecDifference(2) ) )/dNorm;
}
void VoEstimator::publishPose(int64_t utime, std::string channel, Eigen::Isometry3d pose,
Eigen::Vector3d vel_lin, Eigen::Vector3d vel_ang){
Eigen::Quaterniond r(pose.rotation());
bot_core::pose_t pose_msg;
pose_msg.utime = utime;
pose_msg.pos[0] = pose.translation().x();
pose_msg.pos[1] = pose.translation().y();
pose_msg.pos[2] = pose.translation().z();
pose_msg.orientation[0] = r.w();
pose_msg.orientation[1] = r.x();
pose_msg.orientation[2] = r.y();
pose_msg.orientation[3] = r.z();
pose_msg.vel[0] = vel_lin(0);
pose_msg.vel[1] = vel_lin(1);
pose_msg.vel[2] = vel_lin(2);
pose_msg.rotation_rate[0] = vel_ang(0);
pose_msg.rotation_rate[1] = vel_ang(1);
pose_msg.rotation_rate[2] = vel_ang(2);
pose_msg.accel[0]=0; // not estimated or filled in
pose_msg.accel[1]=0;
pose_msg.accel[2]=0;
lcm_->publish( channel, &pose_msg);
}
bool pronto_vis::interpolateScan(const std::vector<float>& iRanges,
const double iTheta0, const double iThetaStep,
const Eigen::Isometry3d& iPose0,
const Eigen::Isometry3d& iPose1,
std::vector<Eigen::Vector3f>& oPoints) {
const int n = iRanges.size();
if (n < 2) return false;
const double tStep = 1.0/(n-1);
Eigen::Quaterniond q0(iPose0.linear());
Eigen::Quaterniond q1(iPose1.linear());
Eigen::Vector3d pos0(iPose0.translation());
Eigen::Vector3d pos1(iPose1.translation());
oPoints.resize(n);
double t = 0;
double theta = iTheta0;
for (int i = 0; i < n; ++i, t += tStep, theta += iThetaStep) {
Eigen::Quaterniond q = q0.slerp(t,q1);
Eigen::Vector3d pos = (1-t)*pos0 + t*pos1;
Eigen::Vector3d pt = iRanges[i]*Eigen::Vector3d(cos(theta), sin(theta), 0);
oPoints[i] = (q*pt + pos).cast<float>();
}
return true;
}
Mapper::PointCloud::Ptr Mapper::generatePointCloud(const RGBDFrame::Ptr &frame)
{
PointCloud::Ptr tmp( new PointCloud() );
if ( frame->pointcloud == nullptr )
{
// point cloud is null ptr
frame->pointcloud = boost::make_shared<PointCloud>();
#pragma omp parallel for
for ( int m=0; m<frame->depth.rows; m+=3 )
{
for ( int n=0; n<frame->depth.cols; n+=3 )
{
ushort d = frame->depth.ptr<ushort>(m)[n];
if (d == 0)
continue;
if (d > max_distance * frame->camera.scale)
continue;
PointT p;
cv::Point3f p_cv = frame->project2dTo3d(n, m);
p.b = frame->rgb.ptr<uchar>(m)[n*3];
p.g = frame->rgb.ptr<uchar>(m)[n*3+1];
p.r = frame->rgb.ptr<uchar>(m)[n*3+2];
p.x = p_cv.x;
p.y = p_cv.y;
p.z = p_cv.z;
frame->pointcloud->points.push_back( p );
}
}
}
Eigen::Isometry3d T = frame->getTransform().inverse();
pcl::transformPointCloud( *frame->pointcloud, *tmp, T.matrix());
tmp->is_dense = false;
return tmp;
}
bool poseEstimationDirect ( const vector< Measurement >& measurements, cv::Mat* gray, Eigen::Matrix3f& K, Eigen::Isometry3d& Tcw )
{
// 初始化g2o
typedef g2o::BlockSolver<g2o::BlockSolverTraits<6,1>> DirectBlock; // 求解的向量 顶点(姿态) 是6＊1的
DirectBlock::LinearSolverType* linearSolver = new g2o::LinearSolverDense< DirectBlock::PoseMatrixType > ();
DirectBlock* solver_ptr = new DirectBlock ( linearSolver );
// g2o::OptimizationAlgorithmGaussNewton* solver = new g2o::OptimizationAlgorithmGaussNewton( solver_ptr ); // G-N 高斯牛顿
g2o::OptimizationAlgorithmLevenberg* solver = new g2o::OptimizationAlgorithmLevenberg ( solver_ptr ); // L-M
g2o::SparseOptimizer optimizer;
optimizer.setAlgorithm ( solver );
optimizer.setVerbose( true );
// 添加顶点
g2o::VertexSE3Expmap* pose = new g2o::VertexSE3Expmap();//位姿
pose->setEstimate ( g2o::SE3Quat ( Tcw.rotation(), Tcw.translation() ) );//旋转矩阵 和 平移向量
pose->setId ( 0 );//id
optimizer.addVertex ( pose );//添加顶点
// 添加边
int id=1;
for ( Measurement m: measurements )
{
EdgeSE3ProjectDirect* edge = new EdgeSE3ProjectDirect (
m.pos_world,//3D 位置
K ( 0,0 ), K ( 1,1 ), K ( 0,2 ), K ( 1,2 ), gray//相机内参数 灰度图
);
edge->setVertex ( 0, pose );//顶点
edge->setMeasurement ( m.grayscale );//测量值为真是灰度值
edge->setInformation ( Eigen::Matrix<double,1,1>::Identity() );//误差 权重 信息矩阵
edge->setId ( id++ );
optimizer.addEdge ( edge );
}
cout<<"边的数量 edges in graph: "<<optimizer.edges().size() <<endl;
optimizer.initializeOptimization();//优化初始化
optimizer.optimize ( 30 );//最大优化次数
Tcw = pose->estimate();// 变换矩阵
}
void compute_velocity(const Eigen::Vector3d& v_parent,
const Eigen::Vector3d& w_parent,
const Eigen::Vector3d& v_rel,
const Eigen::Vector3d& w_rel,
const Eigen::Vector3d& offset,
const Eigen::Isometry3d& tf_parent,
Eigen::Vector3d& v_child,
Eigen::Vector3d& w_child)
{
const Eigen::Matrix3d& R = tf_parent.rotation();
v_child = v_parent + R*v_rel + w_parent.cross(R*offset);
w_child = w_parent + R*w_rel;
}
/**
* @function transf2Params
* @brief Fill par arguments for rot and trans from a _Tf
*/
void transf2Params( const Eigen::Isometry3d &_Tf,
SQ_params &_par ) {
_par.px = _Tf.translation().x();
_par.py = _Tf.translation().y();
_par.pz = _Tf.translation().z();
double r31, r11, r21, r32, r33;
double r, p, y;
r31 = _Tf.linear()(2,0);
r11 = _Tf.linear()(0,0);
r21 = _Tf.linear()(1,0);
r32 = _Tf.linear()(2,1);
r33 = _Tf.linear()(2,2);
p = atan2( -r31, sqrt(r11*r11 + r21*r21) );
y = atan2( r21 / cos(p), r11 / cos(p) );
r = atan2( r32 / cos(p), r33 / cos(p) );
_par.ra = r;
_par.pa = p;
_par.ya = y;
}
//==============================================================================
Eigen::Isometry3d State::getCOMFrame() const
{
Eigen::Isometry3d T = Eigen::Isometry3d::Identity();
// Y-axis
const Eigen::Vector3d yAxis = Eigen::Vector3d::UnitY();
// X-axis
Eigen::Vector3d pelvisXAxis = mPelvis->getTransform().linear().col(0);
const double mag = yAxis.dot(pelvisXAxis);
pelvisXAxis -= mag * yAxis;
const Eigen::Vector3d xAxis = pelvisXAxis.normalized();
// Z-axis
const Eigen::Vector3d zAxis = xAxis.cross(yAxis);
T.translation() = getCOM();
T.linear().col(0) = xAxis;
T.linear().col(1) = yAxis;
T.linear().col(2) = zAxis;
return T;
}
int collideBoxBox(CollisionObject* o1, CollisionObject* o2,
const Eigen::Vector3d& size0, const Eigen::Isometry3d& T0,
const Eigen::Vector3d& size1, const Eigen::Isometry3d& T1,
CollisionResult& result)
{
dVector3 halfSize0;
dVector3 halfSize1;
convVector(0.5 * size0, halfSize0);
convVector(0.5 * size1, halfSize1);
dMatrix3 R0, R1;
convMatrix(T0, R0);
convMatrix(T1, R1);
dVector3 p0;
dVector3 p1;
convVector(T0.translation(), p0);
convVector(T1.translation(), p1);
return dBoxBox(o1, o2, p1, R1, halfSize1, p0, R0, halfSize0, result);
}
void FeatureTransformationEstimator::estimatePoseSVD(Eigen::MatrixXd P, Eigen::MatrixXd Q, Eigen::Isometry3d &T)
{
pcl::TransformationFromCorrespondences tfc;
for (int i = 0; i < P.cols(); i++) {
Eigen::Vector3f p_i = P.col(i).cast<float>();
Eigen::Vector3f q_i = Q.col(i).cast<float>();
float inverse_weight = p_i(2)*p_i(2) + q_i(2)*q_i(2);
float weight = 1;
if (inverse_weight > 0) {
weight = 1. / weight;
}
tfc.add(p_i, q_i, weight);
}
T.matrix() = tfc.getTransformation().matrix().cast<double>();
// T.matrix() = Eigen::umeyama(P, Q, false);
}
PointCloud::Ptr joinPointCloud( PointCloud::Ptr original, FRAME& newFrame, Eigen::Isometry3d T, CAMERA_INTRINSIC_PARAMETERS& camera ){
PointCloud::Ptr newCloud = image2PointCloud(newFrame.rgb,newFrame.depth,camera);
// 合并点云
PointCloud::Ptr output(new PointCloud());
pcl::transformPointCloud(*original,*output,T.matrix());
*newCloud += *output;
// Voxel grid 滤波降采样
static pcl::VoxelGrid<PointT> voxel;
static ParameterReader pd;
double gridsize = atof(pd.getData("voxel_grid").c_str());
voxel.setLeafSize(gridsize,gridsize,gridsize);
voxel.setInputCloud(newCloud);
PointCloud::Ptr tmp(new PointCloud());
voxel.filter(*tmp);
return tmp;
}
void computeProjectiveTransform()
{
// y points downward in the image, upward in real world.
// same for z.
Eigen::Isometry3d to_opencv = Eigen::Isometry3d::Identity();
to_opencv(1,1) = to_opencv(2,2) = -1;
Eigen::Projective3d projection = Eigen::Projective3d::Identity();
if (iface->isOrthographic())
{
projection(2,2) = 0;
projection(2,3) = 1;
}
Eigen::Isometry3d intrinsics = intrinsicsTransform();
inv_camera_transform = camera_transform.inverse();
project_transform = intrinsics * projection * to_opencv * camera_transform;
inv_project_transform = project_transform.inverse();
}
void compute_acceleration(const Eigen::Vector3d& a_parent,
const Eigen::Vector3d& alpha_parent,
const Eigen::Vector3d& w_parent,
const Eigen::Vector3d& a_rel,
const Eigen::Vector3d& alpha_rel,
const Eigen::Vector3d& v_rel,
const Eigen::Vector3d& w_rel,
const Eigen::Vector3d& offset,
const Eigen::Isometry3d& tf_parent,
Eigen::Vector3d& a_child,
Eigen::Vector3d& alpha_child)
{
const Eigen::Matrix3d& R = tf_parent.rotation();
a_child = a_parent + R*a_rel
+ alpha_parent.cross(R*offset)
+ 2*w_parent.cross(R*v_rel)
+ w_parent.cross(w_parent.cross(R*offset));
alpha_child = alpha_parent + R*alpha_rel + w_parent.cross(R*w_rel);
}
Eigen::Isometry3d SmoothEstimatePropagator::SmoothPropagateAction::
exponencialInterpolation(const Eigen::Isometry3d& from, const Eigen::Isometry3d& to, double step) const
{
Eigen::Isometry3d res;
double maxdist = maxDistance-2;
// step goes from 1 to maxDistance, we need x from 0 to 1 in a linear way.
double x = 1 - ((maxdist - (step-1))/maxdist);
// alpha in [0, inf) describes the explonential ramp "steepness"
double alpha = 50;
// exponential ramp from 0 to 1
double exp_ramp = 1 - (x/(1+alpha*(1-x)));
// using quaternion representation and slerp for interpolate between from and to isometry transformations
res.linear() = (Eigen::Quaterniond(from.rotation()).slerp(exp_ramp, Eigen::Quaterniond(to.rotation()))).toRotationMatrix();
res.translation() = (1 - exp_ramp) * from.translation() + exp_ramp * to.translation();
return res;
}
int
main (int argc, char** argv)
{
// 1. Parse arguments:
print_info ("Manually generate a simulated RGB-D point cloud using pcl::simulation. For more information, use: %s -h\n", argv[0]);
if (argc < 3)
{
printHelp (argc, argv);
return (-1);
}
int mode=atoi(argv[1]);
// 2 Construct the simulation method:
int width = 640;
int height = 480;
simexample = SimExample::Ptr (new SimExample (argc, argv, height,width ));
// 3 Generate a series of simulation poses:
// -0 100 fixed poses
// -1 a 'halo' camera
// -2 slerp between two different poses
std::vector<Eigen::Isometry3d, Eigen::aligned_allocator<Eigen::Isometry3d> > poses;
if (mode==0){
// Create a pose:
Eigen::Isometry3d pose;
pose.setIdentity();
Matrix3d m;
//ypr:
m = AngleAxisd(-9.14989, Vector3d::UnitZ()) * AngleAxisd(0.20944, Vector3d::UnitY()) * AngleAxisd(0, Vector3d::UnitX());
pose *= m;
Vector3d v;
v << 1.31762, 0.382931, 1.89533;
pose.translation() = v;
for (int i=0;i< 100;i++){ // duplicate the pose 100 times
poses.push_back(pose);
}
}else if(mode==1){
Eigen::Vector3d focus_center(0,0,1.3);
double halo_r = 4;
double halo_dz = 2;
int n_poses=16;
generate_halo(poses,focus_center,halo_r,halo_dz,n_poses);
}else if (mode==2){
Eigen::Isometry3d pose1;
pose1.setIdentity();
pose1.translation() << 1,0.75,2;
Eigen::Matrix3d rot1;
rot1 = AngleAxisd(M_PI, Eigen::Vector3d::UnitZ())
* AngleAxisd(M_PI/10, Eigen::Vector3d::UnitY())
* AngleAxisd(0.0, Eigen::Vector3d::UnitZ()); // ypr
pose1.rotate(rot1);
Eigen::Isometry3d pose2;
pose2.setIdentity();
pose2.translation() << 1,-1,3;
Eigen::Matrix3d rot2;
rot2 = AngleAxisd(3*M_PI/4, Eigen::Vector3d::UnitZ())
* AngleAxisd(M_PI/4, Eigen::Vector3d::UnitY())
* AngleAxisd(0.0, Eigen::Vector3d::UnitZ()); // ypr
pose2.rotate(rot2);
int n_poses = 20;
for (double i=0; i<=1;i+= 1/((double) n_poses -1) ){
Eigen::Quaterniond rot3;
Eigen::Quaterniond r1(pose1.rotation());
Eigen::Quaterniond r2(pose2.rotation());
rot3 = r1.slerp(i,r2);
Eigen::Isometry3d pose;
pose.setIdentity();
Eigen::Vector3d trans3 = (1-i)*pose1.translation() + i*pose2.translation();
pose.translation() << trans3[0] ,trans3[1] ,trans3[2];
pose.rotate(rot3);
poses.push_back(pose);
}
}
// 4 Do the simulation and write the output:
double tic_main = getTime();
for (size_t i=0;i < poses.size();i++){
std::stringstream ss;
ss.precision(20);
ss << "simcloud_" << i;// << ".pcd";
double tic = getTime();
simexample->doSim(poses[i]);
write_sim_output(ss.str());
cout << (getTime() -tic) << " sec\n";
}
cout << poses.size() << " poses simulated in " << (getTime() -tic_main) << "seconds\n";
cout << (poses.size()/ (getTime() -tic_main) ) << "Hz on average\n";
return 0;
}
int main( int argc, char** argv )
{
// 调用格式：命令 [第一个图] [第二个图]
if (argc != 3)
{
cout<<"Usage: ba_example img1, img2"<<endl;
exit(1);
}
// 读取图像
cv::Mat img1 = cv::imread( argv[1] );
cv::Mat img2 = cv::imread( argv[2] );
// 找到对应点
vector<cv::Point2f> pts1, pts2;
if ( findCorrespondingPoints( img1, img2, pts1, pts2 ) == false )
{
cout<<"匹配点不够！"<<endl;
return 0;
}
cout<<"找到了"<<pts1.size()<<"组对应特征点。"<<endl;
// 构造g2o中的图
// 先构造求解器
g2o::SparseOptimizer optimizer;
// 使用Cholmod中的线性方程求解器
g2o::BlockSolver_6_3::LinearSolverType* linearSolver = new g2o::LinearSolverCholmod<g2o::BlockSolver_6_3::PoseMatrixType> ();
// 6*3 的参数
g2o::BlockSolver_6_3* block_solver = new g2o::BlockSolver_6_3( linearSolver );
// L-M 下降
g2o::OptimizationAlgorithmLevenberg* algorithm = new g2o::OptimizationAlgorithmLevenberg( block_solver );
optimizer.setAlgorithm( algorithm );
optimizer.setVerbose( false );
// 添加节点
// 两个位姿节点
for ( int i=0; i<2; i++ )
{
g2o::VertexSE3Expmap* v = new g2o::VertexSE3Expmap();
v->setId(i);
if ( i == 0)
v->setFixed( true ); // 第一个点固定为零
// 预设值为单位Pose，因为我们不知道任何信息
v->setEstimate( g2o::SE3Quat() );
optimizer.addVertex( v );
}
// 很多个特征点的节点
// 以第一帧为准
for ( size_t i=0; i<pts1.size(); i++ )
{
g2o::VertexSBAPointXYZ* v = new g2o::VertexSBAPointXYZ();
v->setId( 2 + i );
// 由于深度不知道，只能把深度设置为1了
double z = 1;
double x = ( pts1[i].x - cx ) * z / fx;
double y = ( pts1[i].y - cy ) * z / fy;
v->setMarginalized(true);
v->setEstimate( Eigen::Vector3d(x,y,z) );
optimizer.addVertex( v );
}
// 准备相机参数
g2o::CameraParameters* camera = new g2o::CameraParameters( fx, Eigen::Vector2d(cx, cy), 0 );
camera->setId(0);
optimizer.addParameter( camera );
// 准备边
// 第一帧
vector<g2o::EdgeProjectXYZ2UV*> edges;
for ( size_t i=0; i<pts1.size(); i++ )
{
g2o::EdgeProjectXYZ2UV* edge = new g2o::EdgeProjectXYZ2UV();
edge->setVertex( 0, dynamic_cast<g2o::VertexSBAPointXYZ*> (optimizer.vertex(i+2)) );
edge->setVertex( 1, dynamic_cast<g2o::VertexSE3Expmap*> (optimizer.vertex(0)) );
edge->setMeasurement( Eigen::Vector2d(pts1[i].x, pts1[i].y ) );
edge->setInformation( Eigen::Matrix2d::Identity() );
edge->setParameterId(0, 0);
// 核函数
edge->setRobustKernel( new g2o::RobustKernelHuber() );
optimizer.addEdge( edge );
edges.push_back(edge);
}
// 第二帧
for ( size_t i=0; i<pts2.size(); i++ )
{
g2o::EdgeProjectXYZ2UV* edge = new g2o::EdgeProjectXYZ2UV();
edge->setVertex( 0, dynamic_cast<g2o::VertexSBAPointXYZ*> (optimizer.vertex(i+2)) );
edge->setVertex( 1, dynamic_cast<g2o::VertexSE3Expmap*> (optimizer.vertex(1)) );
edge->setMeasurement( Eigen::Vector2d(pts2[i].x, pts2[i].y ) );
edge->setInformation( Eigen::Matrix2d::Identity() );
edge->setParameterId(0,0);
// 核函数
edge->setRobustKernel( new g2o::RobustKernelHuber() );
optimizer.addEdge( edge );
edges.push_back(edge);
}
cout<<"开始优化"<<endl;
optimizer.setVerbose(true);
optimizer.initializeOptimization();
optimizer.optimize(10);
cout<<"优化完毕"<<endl;
//我们比较关心两帧之间的变换矩阵
g2o::VertexSE3Expmap* v = dynamic_cast<g2o::VertexSE3Expmap*>( optimizer.vertex(1) );
Eigen::Isometry3d pose = v->estimate();
cout<<"Pose="<<endl<<pose.matrix()<<endl;
// 以及所有特征点的位置
for ( size_t i=0; i<pts1.size(); i++ )
{
g2o::VertexSBAPointXYZ* v = dynamic_cast<g2o::VertexSBAPointXYZ*> (optimizer.vertex(i+2));
cout<<"vertex id "<<i+2<<", pos = ";
Eigen::Vector3d pos = v->estimate();
cout<<pos(0)<<","<<pos(1)<<","<<pos(2)<<endl;
}
// 估计inlier的个数
int inliers = 0;
for ( auto e:edges )
{
e->computeError();
// chi2 就是 error*\Omega*error, 如果这个数很大，说明此边的值与其他边很不相符
if ( e->chi2() > 1 )
{
cout<<"error = "<<e->chi2()<<endl;
}
else
{
inliers++;
}
}
cout<<"inliers in total points: "<<inliers<<"/"<<pts1.size()+pts2.size()<<endl;
optimizer.save("ba.g2o");
return 0;
}
int main(int /*argc*/, char** /*argv*/) {
Line3D l0;
std::cout << "l0: " << std::endl;
printPlueckerLine(std::cout, l0);
std::cout << std::endl;
Vector6d v;
v << 1.0, 1.0, -0.3, 0.5, 0.2, 0.3;
Line3D l1(v);
l1.normalize();
std::cout << "v: ";
printVector(std::cout, v);
std::cout << std::endl;
std::cout << "l1: " << std::endl;
printPlueckerLine(std::cout, l1);
std::cout << "azimuth: " << g2o::internal::getAzimuth(l1.d()) << std::endl;
std::cout << "elevation: " << g2o::internal::getElevation(l1.d()) << std::endl;
std::cout << std::endl;
Line3D l2(l1);
std::cout << "l2: " << std::endl;
printPlueckerLine(std::cout, l2);
std::cout << std::endl;
Eigen::Vector4d mv = l2.ominus(l1);
Eigen::Quaterniond q(sqrt(1 - mv.head<3>().squaredNorm()), mv.x(), mv.y(), mv.z());
Eigen::Vector3d euler_angles = q.toRotationMatrix().eulerAngles(2, 1, 0);
double yaw = euler_angles[0];
double pitch = euler_angles[1];
double roll = euler_angles[2];
std::cout << "l1 ominus l2: " << roll << " " << pitch << " " << yaw << " " << mv[3] << std::endl;
std::cout << std::endl;
v << 0.0, 0.0, 1.0, 0.5, 0.5, 0.0;
l1 = Line3D(v);
l1.normalize();
std::cout << "l1: " << std::endl;
printPlueckerLine(std::cout, l1);
std::cout << "azimuth: " << g2o::internal::getAzimuth(l1.d()) << std::endl;
std::cout << "elevation: " << g2o::internal::getElevation(l1.d()) << std::endl;
std::cout << std::endl;
v << 0.0, 0.0, 1.0, 0.5, -0.5, 0.0;
l2 = Line3D(v);
l2.normalize();
std::cout << "l2: " << std::endl;
printPlueckerLine(std::cout, l2);
std::cout << "azimuth: " << g2o::internal::getAzimuth(l2.d()) << std::endl;
std::cout << "elevation: " << g2o::internal::getElevation(l2.d()) << std::endl;
std::cout << std::endl;
mv = l2.ominus(l1);
q = Eigen::Quaterniond(sqrt(1 - mv.head<3>().squaredNorm()), mv.x(), mv.y(), mv.z());
euler_angles = q.toRotationMatrix().eulerAngles(2, 1, 0);
yaw = euler_angles[0];
pitch = euler_angles[1];
roll = euler_angles[2];
std::cout << "l1 ominus l2: " << roll << " " << pitch << " " << yaw << " " << mv[3] << std::endl;
std::cout << std::endl;
Line3D l3 = Line3D(l1);
std::cout << "l3: " << std::endl;
printPlueckerLine(std::cout, l3);
l3.oplus(mv);
std::cout << "l3 oplus mv: " << std::endl;
printPlueckerLine(std::cout, l3);
std::cout << std::endl;
std::vector<Line3D> l;
v << 0.0, 0.0, 1.0, 1.0, 0.0, 0.0;
Line3D ll = Line3D(v);
ll.normalize();
l.push_back(ll);
v << 0.0, 0.0, 1.0, 0.0, 1.0, 0.0;
ll = Line3D(v);
ll.normalize();
l.push_back(ll);
v << 0.0, 0.0, 1.0, 0.0, 0.0, 1.0;
ll = Line3D(v);
ll.normalize();
l.push_back(ll);
for(size_t i = 0; i < l.size(); ++i) {
Line3D& line = l[i];
std::cout << "line: "
<< line.d()[0] << " " << line.d()[1] << " " << line.d()[2] << " "
<< line.w()[0] << " " << line.w()[1] << " " << line.w()[2] << std::endl;
}
std::cout << std::endl;
Eigen::Isometry3d T = Eigen::Isometry3d::Identity();
T.translation().x() = 0.9;
std::cout << "R: " << std::endl << T.linear() << std::endl;
std::cout << "t: " << std::endl << T.translation() << std::endl;
std::cout << std::endl;
for(size_t i = 0; i < l.size(); ++i) {
Line3D& line = l[i];
line = T * line;
std::cout << "line: "
<< line.d()[0] << " " << line.d()[1] << " " << line.d()[2] << " "
<< line.w()[0] << " " << line.w()[1] << " " << line.w()[2] << std::endl;
}
std::cout << std::endl;
return 0;
}
void GraphicEnd::lostRecovery()
{
//以present作为新的关键帧
cout<<BOLDYELLOW<<"Lost Recovery..."<<RESET<<endl;
//把present中的数据存储到current中
_currKF.id ++;
_currKF.planes = _present.planes;
_currKF.frame_index = _index;
_lastRGB = _currRGB.clone();
_kf_pos = _robot;
//waitKey(0);
_keyframes.push_back( _currKF );
//将current关键帧存储至全局优化器
SparseOptimizer& opt = _pSLAMEnd->globalOptimizer;
//顶点
VertexSE3* v = new VertexSE3();
v->setId( _currKF.id );
if (_use_odometry)
v->setEstimate( _odo_this );
else
v->setEstimate( Eigen::Isometry3d::Identity() );
opt.addVertex( v );
//由于当前位置不知道,所以不添加与上一帧相关的边
if (_use_odometry)
{
Eigen::Isometry3d To = _odo_last.inverse()*_odo_this;
EdgeSE3* edge = new EdgeSE3();
edge->vertices()[0] = opt.vertex( _currKF.id - 1 );
edge->vertices()[1] = opt.vertex( _currKF.id );
Eigen::Matrix<double, 6,6> information = Eigen::Matrix<double, 6, 6>::Identity();
information(0, 0) = information(2,2) = information(1, 1) = information(3,3) = information(4,4) = information(5,5) = 1/(_error_odometry*_error_odometry);
edge->setInformation( information );
edge->setMeasurement( To );
opt.addEdge( edge );
_odo_last = _odo_this;
_lost = 0;
return;
}
//check loop closure
for (int i=0; i<_keyframes.size() - 1; i++)
{
vector<PLANE>& p1 = _keyframes[ i ].planes;
Eigen::Isometry3d T = multiPnP( p1, _currKF.planes ).T;
if (T.matrix() == Eigen::Isometry3d::Identity().matrix()) //匹配不上
continue;
T = T.inverse();
//若匹配上，则在两个帧之间加一条边
EdgeSE3* edge = new EdgeSE3();
edge->vertices() [0] = opt.vertex( _keyframes[i].id );
edge->vertices() [1] = opt.vertex( _currKF.id );
Eigen::Matrix<double, 6,6> information = Eigen::Matrix<double, 6, 6>::Identity();
information(0, 0) = information(1,1) = information(2,2) = 100;
information(3,3) = information(4,4) = information(5,5) = 100;
edge->setInformation( information );
edge->setMeasurement( T );
edge->setRobustKernel( _pSLAMEnd->_robustKernel );
opt.addEdge( edge );
}
}
int main()
{
double euc_noise = 0.01; // noise in position, m
// double outlier_ratio = 0.1;
SparseOptimizer optimizer;
optimizer.setVerbose(false);
// variable-size block solver
BlockSolverX::LinearSolverType * linearSolver = new LinearSolverDense<g2o::BlockSolverX::PoseMatrixType>();
BlockSolverX * solver_ptr = new BlockSolverX(linearSolver);
g2o::OptimizationAlgorithmLevenberg* solver = new g2o::OptimizationAlgorithmLevenberg(solver_ptr);
optimizer.setAlgorithm(solver);
vector<Vector3d> true_points;
for (size_t i=0;i<1000; ++i)
{
true_points.push_back(Vector3d((Sample::uniform()-0.5)*3,
Sample::uniform()-0.5,
Sample::uniform()+10));
}
// set up two poses
int vertex_id = 0;
for (size_t i=0; i<2; ++i)
{
// set up rotation and translation for this node
Vector3d t(0,0,i);
Quaterniond q;
q.setIdentity();
Eigen::Isometry3d cam; // camera pose
cam = q;
cam.translation() = t;
// set up node
VertexSE3 *vc = new VertexSE3();
vc->setEstimate(cam);
vc->setId(vertex_id); // vertex id
cerr << t.transpose() << " | " << q.coeffs().transpose() << endl;
// set first cam pose fixed
if (i==0)
vc->setFixed(true);
// add to optimizer
optimizer.addVertex(vc);
vertex_id++;
}
// set up point matches
for (size_t i=0; i<true_points.size(); ++i)
{
// get two poses
VertexSE3* vp0 =
dynamic_cast<VertexSE3*>(optimizer.vertices().find(0)->second);
VertexSE3* vp1 =
dynamic_cast<VertexSE3*>(optimizer.vertices().find(1)->second);
// calculate the relative 3D position of the point
Vector3d pt0,pt1;
pt0 = vp0->estimate().inverse() * true_points[i];
pt1 = vp1->estimate().inverse() * true_points[i];
// add in noise
pt0 += Vector3d(Sample::gaussian(euc_noise ),
Sample::gaussian(euc_noise ),
Sample::gaussian(euc_noise ));
pt1 += Vector3d(Sample::gaussian(euc_noise ),
Sample::gaussian(euc_noise ),
Sample::gaussian(euc_noise ));
// form edge, with normals in varioius positions
Vector3d nm0, nm1;
nm0 << 0, i, 1;
nm1 << 0, i, 1;
nm0.normalize();
nm1.normalize();
Edge_V_V_GICP * e // new edge with correct cohort for caching
= new Edge_V_V_GICP();
e->setVertex(0, vp0); // first viewpoint
e->setVertex(1, vp1); // second viewpoint
EdgeGICP meas;
meas.pos0 = pt0;
meas.pos1 = pt1;
meas.normal0 = nm0;
meas.normal1 = nm1;
e->setMeasurement(meas);
// e->inverseMeasurement().pos() = -kp;
meas = e->measurement();
// use this for point-plane
e->information() = meas.prec0(0.01);
// use this for point-point
// e->information().setIdentity();
// e->setRobustKernel(true);
//e->setHuberWidth(0.01);
optimizer.addEdge(e);
}
// move second cam off of its true position
VertexSE3* vc =
dynamic_cast<VertexSE3*>(optimizer.vertices().find(1)->second);
Eigen::Isometry3d cam = vc->estimate();
cam.translation() = Vector3d(0,0,0.2);
vc->setEstimate(cam);
optimizer.initializeOptimization();
optimizer.computeActiveErrors();
cout << "Initial chi2 = " << FIXED(optimizer.chi2()) << endl;
optimizer.setVerbose(true);
optimizer.optimize(5);
cout << endl << "Second vertex should be near 0,0,1" << endl;
cout << dynamic_cast<VertexSE3*>(optimizer.vertices().find(0)->second)
->estimate().translation().transpose() << endl;
cout << dynamic_cast<VertexSE3*>(optimizer.vertices().find(1)->second)
->estimate().translation().transpose() << endl;
}
int GraphicEnd::run()
{
//清空present并读取新的数据
cout<<"********************"<<endl;
_present.planes.clear();
readimage();
//处理present
_present.planes = extractPlanesAndGenerateImage( _currCloud, _currRGB, _currDep );
//_present.planes = extractPlanes( _currCloud );
//generateImageOnPlane( _currRGB, _present.planes, _currDep );
for ( size_t i=0; i<_present.planes.size(); i++ )
{
_present.planes[i].kp = extractKeypoints( _present.planes[i].image );
_present.planes[i].desp = extractDescriptor( _currRGB, _present.planes[i].kp );
compute3dPosition( _present.planes[i], _currDep);
}
// 求解present到current的变换矩阵
RESULT_OF_MULTIPNP result = multiPnP( _currKF.planes, _present.planes );
Eigen::Isometry3d T = result.T;
T = T.inverse(); //好像是反着的
// 如果平移和旋转超过一个阈值，则定义新的关键帧
if ( T.matrix() == Eigen::Isometry3d::Identity().matrix() )
{
//未匹配上
cout<<BOLDRED"This frame lost"<<RESET<<endl;
_lost++;
}
else if (result.norm > _max_pos_change)
{
//生成一个新的关键帧
_robot = T * _kf_pos;
generateKeyFrame(T);
if (_loop_closure_detection == true)
loopClosure();
_lost = 0;
}
else
{
//小变化，更新robot位置
_robot = T * _kf_pos;
_lost = 0;
}
if (_lost > _lost_frames)
{
cerr<<"the robot lost. Perform lost recovery."<<endl;
lostRecovery();
}
cout<<RED<<"key frame size = "<<_keyframes.size()<<RESET<<endl;
_index ++;
if (_use_odometry)
{
_odo_this = _odometry[ _index-1 ];
}
return 1;
}
//回环检测：在过去的帧中随机取_loopclosure_frames那么多帧进行两两比较
void GraphicEnd::loopClosure()
{
if (_keyframes.size() <= 3 ) //小于3时，回环没有意义
return;
cout<<"Checking loop closure."<<endl;
waitKey(10);
vector<int> checked;
SparseOptimizer& opt = _pSLAMEnd->globalOptimizer;
//相邻帧
for (int i=-3; i>-5; i--)
{
int n = _keyframes.size() + i;
if (n>=0)
{
vector<PLANE>& p1 = _keyframes[n].planes;
Eigen::Isometry3d T = multiPnP( p1, _currKF.planes ).T;
if (T.matrix() == Eigen::Isometry3d::Identity().matrix()) //匹配不上
continue;
T = T.inverse();
//若匹配上，则在两个帧之间加一条边
EdgeSE3* edge = new EdgeSE3();
edge->vertices() [0] = opt.vertex( _keyframes[n].id );
edge->vertices() [1] = opt.vertex( _currKF.id );
Eigen::Matrix<double, 6,6> information = Eigen::Matrix<double, 6, 6>::Identity();
information(0, 0) = information(2,2) = 100;
information(1,1) = 100;
information(3,3) = information(4,4) = information(5,5) = 100;
edge->setInformation( information );
edge->setMeasurement( T );
edge->setRobustKernel( _pSLAMEnd->_robustKernel );
opt.addEdge( edge );
}
else
break;
}
//搜索种子帧
cout<<"checking seeds, seed.size()"<<_seed.size()<<endl;
vector<int> newseed;
for (size_t i=0; i<_seed.size(); i++)
{
vector<PLANE>& p1 = _keyframes[_seed[i]].planes;
Eigen::Isometry3d T = multiPnP( p1, _currKF.planes ).T;
if (T.matrix() == Eigen::Isometry3d::Identity().matrix()) //匹配不上
continue;
T = T.inverse();
//若匹配上，则在两个帧之间加一条边
checked.push_back( _seed[i] );
newseed.push_back( _seed[i] );
EdgeSE3* edge = new EdgeSE3();
edge->vertices() [0] = opt.vertex( _keyframes[_seed[i]].id );
edge->vertices() [1] = opt.vertex( _currKF.id );
Eigen::Matrix<double, 6,6> information = Eigen::Matrix<double, 6, 6>::Identity();
information(0, 0) = information(2,2) = 100;
information(1,1) = 100;
information(3,3) = information(4,4) = information(5,5) = 100;
edge->setInformation( information );
edge->setMeasurement( T );
edge->setRobustKernel( _pSLAMEnd->_robustKernel );
opt.addEdge( edge );
}
//随机搜索
cout<<"checking random frames"<<endl;
for (int i=0; i<_loopclosure_frames; i++)
{
int frame = rand() % (_keyframes.size() -3 ); //随机在过去的帧中取一帧
if ( find(checked.begin(), checked.end(), frame) != checked.end() ) //之前已检查过
continue;
checked.push_back( frame );
vector<PLANE>& p1 = _keyframes[frame].planes;
RESULT_OF_MULTIPNP result = multiPnP( p1, _currKF.planes, true, _keyframes[frame].frame_index, 20 );
Eigen::Isometry3d T = result.T;
if (T.matrix() == Eigen::Isometry3d::Identity().matrix()) //匹配不上
continue;
T = T.inverse();
displayLC( _keyframes[frame].frame_index, _currKF.frame_index, result.norm);
newseed.push_back( frame );
//若匹配上，则在两个帧之间加一条边
cout<<BOLDBLUE<<"find a loop closure between kf "<<_currKF.id<<" and kf "<<frame<<RESET<<endl;
EdgeSE3* edge = new EdgeSE3();
edge->vertices() [0] = opt.vertex( _keyframes[frame].id );
edge->vertices() [1] = opt.vertex( _currKF.id );
Eigen::Matrix<double, 6,6> information = Eigen::Matrix<double, 6, 6>::Identity();
information(0, 0) = information(2,2) = 100;
information(1,1) = 100;
information(3,3) = information(4,4) = information(5,5) = 100;
edge->setInformation( information );
edge->setMeasurement( T );
edge->setRobustKernel( _pSLAMEnd->_robustKernel );
opt.addEdge( edge );
}
waitKey(10);
_seed = newseed;
}
void test_relative_values(bool spatial_targets, bool spatial_followers)
{
const double tolerance = 1e-8;
// These frames will form a chain
SimpleFrame A(Frame::World(), "A");
SimpleFrame B(&A, "B");
SimpleFrame C(&B, "C");
SimpleFrame D(&C, "D");
std::vector<SimpleFrame*> targets;
targets.push_back(&A);
targets.push_back(&B);
targets.push_back(&C);
targets.push_back(&D);
// R will be an arbitrary reference frame
SimpleFrame R(Frame::World(), "R");
targets.push_back(&R);
// Each of these frames will attempt to track one of the frames in the chain
// with respect to the frame R.
SimpleFrame AR(&R, "AR");
SimpleFrame BR(&R, "BR");
SimpleFrame CR(&R, "CR");
SimpleFrame DR(&R, "DR");
SimpleFrame RR(&R, "RR");
std::vector<SimpleFrame*> followers;
followers.push_back(&AR);
followers.push_back(&BR);
followers.push_back(&CR);
followers.push_back(&DR);
followers.push_back(&RR);
assert( targets.size() == followers.size() );
randomize_target_values(targets, spatial_targets);
set_relative_values(targets, followers, spatial_followers);
check_world_values(targets, followers, tolerance);
// Check every combination of relative values
for(std::size_t i=0; i<targets.size(); ++i)
{
Frame* T = targets[i];
for(std::size_t j=0; j<followers.size(); ++j)
{
Frame* F = followers[j];
check_values(targets, followers, T, F, tolerance);
check_values(targets, followers, F, T, tolerance);
check_offset_computations(targets, followers, T, F, tolerance);
}
}
// Test SimpleFrame::setTransform()
for(std::size_t i=0; i<followers.size(); ++i)
{
for(std::size_t j=0; j<followers.size(); ++j)
{
SimpleFrame* F = followers[i];
SimpleFrame* T = followers[j];
Eigen::Isometry3d tf;
randomize_transform(tf, 1, 2*M_PI);
T->setTransform(tf, F);
if(i != j)
EXPECT_TRUE( equals(T->getTransform(F).matrix(), tf.matrix(), 1e-10));
}
}
}
int collideSphereBox(const double& r0, const Eigen::Isometry3d& T0,
const Eigen::Vector3d& size1, const Eigen::Isometry3d& T1,
std::vector<Contact>* result)
{
Eigen::Vector3d size = 0.5 * size1;
bool inside_box = true;
// clipping a center of the sphere to a boundary of the box
Eigen::Vector3d c0 = T0.translation();
Eigen::Vector3d p = T1.inverse() * c0;
if (p[0] < -size[0]) { p[0] = -size[0]; inside_box = false; }
if (p[0] > size[0]) { p[0] = size[0]; inside_box = false; }
if (p[1] < -size[1]) { p[1] = -size[1]; inside_box = false; }
if (p[1] > size[1]) { p[1] = size[1]; inside_box = false; }
if (p[2] < -size[2]) { p[2] = -size[2]; inside_box = false; }
if (p[2] > size[2]) { p[2] = size[2]; inside_box = false; }
Eigen::Vector3d normal(0.0, 0.0, 0.0);
double penetration;
if ( inside_box )
{
// find nearest side from the sphere center
double min = size[0] - std::abs(p[0]);
double tmin = size[1] - std::abs(p[1]);
int idx = 0;
if ( tmin < min )
{
min = tmin;
idx = 1;
}
tmin = size[2] - std::abs(p[2]);
if ( tmin < min )
{
min = tmin;
idx = 2;
}
normal[idx] = (p[idx] > 0.0 ? 1.0 : -1.0);
normal = T1.linear() * normal;
penetration = min + r0;
Contact contact;
contact.point = c0;
contact.normal = normal;
contact.penetrationDepth = penetration;
result->push_back(contact);
return 1;
}
Eigen::Vector3d contactpt = T1 * p;
normal = c0 - contactpt;
double mag = normal.norm();
penetration = r0 - mag;
if (penetration < 0.0)
{
return 0;
}
if (mag > DART_COLLISION_EPS)
{
normal *= (1.0/mag);
Contact contact;
contact.point = contactpt;
contact.normal = normal;
contact.penetrationDepth = penetration;
result->push_back(contact);}
else
{
double min = size[0] - std::abs(p[0]);
double tmin = size[1] - std::abs(p[1]);
int idx = 0;
if ( tmin < min )
{
min = tmin;
idx = 1;
}
tmin = size[2] - std::abs(p[2]);
if ( tmin < min )
{
min = tmin;
idx = 2;
}
normal.setZero();
normal[idx] = (p[idx] > 0.0 ? 1.0 : -1.0);
normal = T1.linear() * normal;
Contact contact;
contact.point = contactpt;
contact.normal = normal;
contact.penetrationDepth = penetration;
result->push_back(contact);
}
return 1;
}
void check_offset_computations(const std::vector<SimpleFrame*>& targets,
const std::vector<SimpleFrame*>& followers,
const Frame* relativeTo,
const Frame* inCoordinatesOf,
double tolerance)
{
for(std::size_t i=0; i<targets.size(); ++i)
{
Frame* T = targets[i];
Frame* F = followers[i];
Eigen::Isometry3d coordTf = relativeTo->getTransform(inCoordinatesOf);
Vector3d offset_T = random_vec<3>();
Vector3d offset_F = T->getTransform(F) * offset_T;
Vector3d v_TO, w_TO, v_FO, w_FO, a_TO, alpha_TO, a_FO, alpha_FO;
// Compute velocity of the offfset in the relative frame
compute_velocity(T->getLinearVelocity(relativeTo, relativeTo),
T->getAngularVelocity(relativeTo, relativeTo),
Eigen::Vector3d::Zero(), Eigen::Vector3d::Zero(),
offset_T, T->getTransform(relativeTo), v_TO, w_TO);
// Convert to the desired coordinate system
v_TO = coordTf.linear() * v_TO;
w_TO = coordTf.linear() * w_TO;
// Compute acceleration of the offset in the relative frame
compute_acceleration(T->getLinearAcceleration(relativeTo, relativeTo),
T->getAngularAcceleration(relativeTo, relativeTo),
T->getAngularVelocity(relativeTo, relativeTo),
Eigen::Vector3d::Zero(), Eigen::Vector3d::Zero(),
Eigen::Vector3d::Zero(), Eigen::Vector3d::Zero(),
offset_T, T->getTransform(relativeTo), a_TO, alpha_TO);
// Convert to the desired coordinate system
a_TO = coordTf.linear() * a_TO;
alpha_TO = coordTf.linear() * alpha_TO;
compute_velocity(F->getLinearVelocity(relativeTo, relativeTo),
F->getAngularVelocity(relativeTo, relativeTo),
Eigen::Vector3d::Zero(), Eigen::Vector3d::Zero(),
offset_F, F->getTransform(relativeTo), v_FO, w_FO);
v_FO = coordTf.linear() * v_FO;
w_FO = coordTf.linear() * w_FO;
compute_acceleration(F->getLinearAcceleration(relativeTo, relativeTo),
F->getAngularAcceleration(relativeTo, relativeTo),
F->getAngularVelocity(relativeTo, relativeTo),
Eigen::Vector3d::Zero(), Eigen::Vector3d::Zero(),
Eigen::Vector3d::Zero(), Eigen::Vector3d::Zero(),
offset_F, F->getTransform(relativeTo), a_FO, alpha_FO);
a_FO = coordTf.linear() * a_FO;
alpha_FO = coordTf.linear() * alpha_FO;
Eigen::Vector3d v_TO_actual = T->getLinearVelocity(offset_T, relativeTo,
inCoordinatesOf);
Eigen::Vector3d v_FO_actual = F->getLinearVelocity(offset_F, relativeTo,
inCoordinatesOf);
EXPECT_TRUE( equals(v_TO, v_FO, tolerance) );
EXPECT_TRUE( equals(v_TO, v_TO_actual, tolerance) );
EXPECT_TRUE( equals(v_FO, v_FO_actual, tolerance) );
Eigen::Vector3d a_TO_actual = T->getLinearAcceleration(offset_T, relativeTo,
inCoordinatesOf);
Eigen::Vector3d a_FO_actual = F->getLinearAcceleration(offset_F, relativeTo,
inCoordinatesOf);
EXPECT_TRUE( equals(a_TO, a_FO, tolerance) );
EXPECT_TRUE( equals(a_TO, a_TO_actual, tolerance) );
EXPECT_TRUE( equals(a_FO, a_FO_actual, tolerance) );
}
}
TEST(FRAMES, FORWARD_KINEMATICS_CHAIN)
{
std::vector<SimpleFrame*> frames;
double tolerance = 1e-6;
SimpleFrame A(Frame::World(), "A");
frames.push_back(&A);
SimpleFrame B(&A, "B");
frames.push_back(&B);
SimpleFrame C(&B, "C");
frames.push_back(&C);
SimpleFrame D(&C, "D");
frames.push_back(&D);
// -- Test Position --------------------------------------------------------
EXPECT_TRUE( equals(D.getTransform().matrix(),
Eigen::Isometry3d::Identity().matrix(),
tolerance));
Eigen::aligned_vector<Eigen::Isometry3d> tfs;
tfs.resize(frames.size(), Eigen::Isometry3d::Identity());
randomize_transforms(tfs);
for(std::size_t i=0; i<frames.size(); ++i)
{
SimpleFrame* F = frames[i];
F->setRelativeTransform(tfs[i]);
}
for(std::size_t i=0; i<frames.size(); ++i)
{
Frame* F = frames[i];
Eigen::Isometry3d expectation(Eigen::Isometry3d::Identity());
for(std::size_t j=0; j<=i; ++j)
{
expectation = expectation * tfs[j];
}
Eigen::Isometry3d actual = F->getTransform();
EXPECT_TRUE( equals(actual.matrix(), expectation.matrix(), tolerance));
}
randomize_transforms(tfs);
for(std::size_t i=0; i<frames.size(); ++i)
{
SimpleFrame* F = frames[i];
F->setRelativeTransform(tfs[i]);
}
Eigen::Isometry3d expectation(Eigen::Isometry3d::Identity());
for(std::size_t j=0; j<frames.size(); ++j)
expectation = expectation * tfs[j];
EXPECT_TRUE( equals(frames.back()->getTransform().matrix(),
expectation.matrix(),
tolerance) );
// -- Test Velocity --------------------------------------------------------
// Basic forward spatial velocity propagation
{ // The brackets are to allow reusing variable names
Eigen::aligned_vector<Eigen::Vector6d> v_rels(frames.size());
Eigen::aligned_vector<Eigen::Vector6d> v_total(frames.size());
for(std::size_t i=0; i<frames.size(); ++i)
{
v_rels[i] = random_vec<6>();
SimpleFrame* F = frames[i];
F->setRelativeSpatialVelocity(v_rels[i]);
if(i>0)
compute_spatial_velocity(v_total[i-1], v_rels[i],
F->getRelativeTransform(), v_total[i]);
else
compute_spatial_velocity(Eigen::Vector6d::Zero(), v_rels[i],
F->getRelativeTransform(), v_total[i]);
}
for(std::size_t i=0; i<frames.size(); ++i)
{
SimpleFrame* F = frames[i];
Eigen::Vector6d v_actual = F->getSpatialVelocity();
EXPECT_TRUE( equals(v_total[i], v_actual) );
}
}
// Testing conversion beteween spatial and classical velocities
{
std::vector<Eigen::Vector3d> v_rels(frames.size());
std::vector<Eigen::Vector3d> w_rels(frames.size());
std::vector<Eigen::Vector3d> v_total(frames.size());
std::vector<Eigen::Vector3d> w_total(frames.size());
for(std::size_t i=0; i<frames.size(); ++i)
{
v_rels[i] = random_vec<3>();
w_rels[i] = random_vec<3>();
SimpleFrame* F = frames[i];
F->setClassicDerivatives(v_rels[i], w_rels[i]);
Eigen::Vector3d offset = F->getRelativeTransform().translation();
Eigen::Isometry3d tf = i>0? frames[i-1]->getTransform() :
Eigen::Isometry3d::Identity();
if(i>0)
compute_velocity(v_total[i-1], w_total[i-1], v_rels[i], w_rels[i],
offset, tf, v_total[i], w_total[i]);
else
compute_velocity(Eigen::Vector3d::Zero(), Eigen::Vector3d::Zero(),
v_rels[i], w_rels[i],
offset, tf, v_total[i], w_total[i]);
}
for(std::size_t i=0; i<frames.size(); ++i)
{
SimpleFrame* F = frames[i];
Eigen::Vector3d v_actual = F->getLinearVelocity();
Eigen::Vector3d w_actual = F->getAngularVelocity();
EXPECT_TRUE( equals(v_total[i], v_actual, tolerance) );
EXPECT_TRUE( equals(w_total[i], w_actual, tolerance) );
}
}
// -- Acceleration ---------------------------------------------------------
// Basic forward spatial acceleration propagation
{
Eigen::aligned_vector<Eigen::Vector6d> v_rels(frames.size());
Eigen::aligned_vector<Eigen::Vector6d> a_rels(frames.size());
Eigen::aligned_vector<Eigen::Vector6d> v_total(frames.size());
Eigen::aligned_vector<Eigen::Vector6d> a_total(frames.size());
for(std::size_t i=0; i<frames.size(); ++i)
{
v_rels[i] = random_vec<6>();
a_rels[i] = random_vec<6>();
SimpleFrame* F = frames[i];
F->setRelativeSpatialVelocity(v_rels[i]);
F->setRelativeSpatialAcceleration(a_rels[i]);
Eigen::Isometry3d tf = F->getRelativeTransform();
if(i>0)
{
compute_spatial_velocity(v_total[i-1], v_rels[i], tf, v_total[i]);
compute_spatial_acceleration(a_total[i-1], a_rels[i], v_total[i],
v_rels[i], tf, a_total[i]);
}
else
{
compute_spatial_velocity(Eigen::Vector6d::Zero(), v_rels[i],
tf, v_total[i]);
compute_spatial_acceleration(Eigen::Vector6d::Zero(), a_rels[i],
v_total[i], v_rels[i], tf, a_total[i]);
}
}
for(std::size_t i=0; i<frames.size(); ++i)
{
SimpleFrame* F = frames[i];
Eigen::Vector6d a_actual = F->getSpatialAcceleration();
EXPECT_TRUE( equals(a_total[i], a_actual) );
}
// Test relative computations
for(std::size_t i=0; i<frames.size(); ++i)
{
Frame* F = frames[i];
Frame* P = F->getParentFrame();
Eigen::Vector6d v_rel = F->getSpatialVelocity(P, F);
Eigen::Vector6d a_rel = F->getSpatialAcceleration(P, F);
EXPECT_TRUE( equals(v_rels[i], v_rel, tolerance) );
EXPECT_TRUE( equals(a_rels[i], a_rel, tolerance) );
}
// Test offset computations
for(std::size_t i=0; i<frames.size(); ++i)
{
Eigen::Vector3d offset = random_vec<3>();
Frame* F = frames[i];
Eigen::Vector3d v_actual = F->getLinearVelocity(offset);
Eigen::Vector3d w_actual = F->getAngularVelocity();
Eigen::Vector3d v_expect, w_expect;
compute_velocity(F->getLinearVelocity(), F->getAngularVelocity(),
Eigen::Vector3d::Zero(), Eigen::Vector3d::Zero(),
offset, F->getWorldTransform(), v_expect, w_expect);
EXPECT_TRUE( equals( v_expect, v_actual) );
EXPECT_TRUE( equals( w_expect, w_actual) );
Eigen::Vector3d a_actual = F->getLinearAcceleration(offset);
Eigen::Vector3d alpha_actual = F->getAngularAcceleration();
Eigen::Vector3d a_expect, alpha_expect;
compute_acceleration(F->getLinearAcceleration(),
F->getAngularAcceleration(), F->getAngularVelocity(),
Eigen::Vector3d::Zero(), Eigen::Vector3d::Zero(),
Eigen::Vector3d::Zero(), Eigen::Vector3d::Zero(),
offset, F->getWorldTransform(),
a_expect, alpha_expect);
EXPECT_TRUE( equals( a_expect, a_actual) );
EXPECT_TRUE( equals( alpha_expect, alpha_actual) );
}
}
// Testing conversion between spatial and classical accelerations
{
std::vector<Eigen::Vector3d> v_rels(frames.size());
std::vector<Eigen::Vector3d> w_rels(frames.size());
std::vector<Eigen::Vector3d> a_rels(frames.size());
std::vector<Eigen::Vector3d> alpha_rels(frames.size());
std::vector<Eigen::Vector3d> v_total(frames.size());
std::vector<Eigen::Vector3d> w_total(frames.size());
std::vector<Eigen::Vector3d> a_total(frames.size());
std::vector<Eigen::Vector3d> alpha_total(frames.size());
for(std::size_t i=0; i<frames.size(); ++i)
{
v_rels[i] = random_vec<3>();
w_rels[i] = random_vec<3>();
a_rels[i] = random_vec<3>();
alpha_rels[i] = random_vec<3>();
SimpleFrame* F = frames[i];
Eigen::Isometry3d rel_tf;
randomize_transform(rel_tf);
F->setRelativeTransform(rel_tf);
F->setClassicDerivatives(v_rels[i], w_rels[i], a_rels[i], alpha_rels[i]);
Eigen::Vector3d offset = F->getRelativeTransform().translation();
Eigen::Isometry3d tf = i>0? frames[i-1]->getTransform() :
Eigen::Isometry3d::Identity();
if(i>0)
{
compute_velocity(v_total[i-1], w_total[i-1], v_rels[i], w_rels[i],
offset, tf, v_total[i], w_total[i]);
compute_acceleration(a_total[i-1], alpha_total[i-1], w_total[i-1],
a_rels[i], alpha_rels[i], v_rels[i], w_rels[i], offset, tf,
a_total[i], alpha_total[i]);
}
else
{
compute_velocity(Eigen::Vector3d::Zero(), Eigen::Vector3d::Zero(),
v_rels[i], w_rels[i], offset, tf,
v_total[i], w_total[i]);
compute_acceleration(Eigen::Vector3d::Zero(), Eigen::Vector3d::Zero(),
Eigen::Vector3d::Zero(), a_rels[i], alpha_rels[i],
v_rels[i], w_rels[i], offset, tf,
a_total[i], alpha_total[i]);
}
}
for(std::size_t i=0; i<frames.size(); ++i)
{
SimpleFrame* F = frames[i];
Eigen::Vector3d v_actual = F->getLinearVelocity();
Eigen::Vector3d w_actual = F->getAngularVelocity();
Eigen::Vector3d a_actual = F->getLinearAcceleration();
Eigen::Vector3d alpha_actual = F->getAngularAcceleration();
EXPECT_TRUE( equals(v_total[i], v_actual, tolerance) );
EXPECT_TRUE( equals(w_total[i], w_actual, tolerance) );
EXPECT_TRUE( equals(a_total[i], a_actual, tolerance) );
EXPECT_TRUE( equals(alpha_total[i], alpha_actual, tolerance) );
}
// Test relative computations
for(std::size_t i=0; i<frames.size(); ++i)
{
SimpleFrame* F = frames[i];
Frame* P = F->getParentFrame();
Eigen::Vector3d v_rel = F->getLinearVelocity(P, P);
Eigen::Vector3d w_rel = F->getAngularVelocity(P, P);
Eigen::Vector3d a_rel = F->getLinearAcceleration(P, P);
Eigen::Vector3d alpha_rel = F->getAngularAcceleration(P, P);
EXPECT_TRUE( equals(v_rels[i], v_rel, tolerance) );
EXPECT_TRUE( equals(w_rels[i], w_rel, tolerance) );
EXPECT_TRUE( equals(a_rels[i], a_rel, tolerance) );
EXPECT_TRUE( equals(alpha_rels[i], alpha_rel, tolerance) );
}
}
}
void ViewerState::optimizeSelected(){
DrawableFrame* current = drawableFrameVector.back();
DrawableFrame* reference = drawableFrameVector[drawableFrameVector.size()-2];
cerr << "optimizing" << endl;
cerr << "current=" << current->frame() << endl;
cerr << "reference= " << reference->frame() << endl;
// cerr computing initial guess based on the frame positions, just for convenience
Eigen::Isometry3d delta = reference->_vertex->estimate().inverse()*current->_vertex->estimate();
for(int c=0; c<4; c++)
for(int r=0; r<3; r++)
initialGuess.matrix()(r,c) = delta.matrix()(r,c);
Eigen::Isometry3f odometryMean;
Matrix6f odometryInfo;
bool hasOdometry = extractRelativePrior(odometryMean, odometryInfo, reference, current);
if (hasOdometry)
initialGuess=odometryMean;
Eigen::Isometry3f imuMean;
Matrix6f imuInfo;
bool hasImu = extractAbsolutePrior(imuMean, imuInfo, current);
initialGuess.matrix().row(3) << 0,0,0,1;
if(!wasInitialGuess) {
aligner->clearPriors();
aligner->setOuterIterations(al_outerIterations);
aligner->setReferenceFrame(reference->frame());
aligner->setCurrentFrame(current->frame());
aligner->setInitialGuess(initialGuess);
aligner->setSensorOffset(sensorOffset);
if (hasOdometry)
aligner->addRelativePrior(odometryMean, odometryInfo);
if (hasImu)
aligner->addAbsolutePrior(reference->transformation(), imuMean, imuInfo);
aligner->align();
}
Eigen::Isometry3f localTransformation =aligner->T();
if (aligner->inliers()<1000 || aligner->error()/aligner->inliers()>10){
cerr << "!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!" << endl;
cerr << "aligner: monster failure: inliers = " << aligner->inliers() << endl;
cerr << "aligner: monster failure: error/inliers = " << aligner->error()/aligner->inliers() << endl;
cerr << "Local transformation: " << t2v(aligner->T()).transpose() << endl;
localTransformation = initialGuess;
sleep(1);
}
cout << "Local transformation: " << t2v(aligner->T()).transpose() << endl;
globalT = reference->transformation()*aligner->T();
// Update cloud drawing position.
current->setTransformation(globalT);
current->setLocalTransformation(aligner->T());
// Show zBuffers.
refScn->clear();
currScn->clear();
QImage refQImage;
QImage currQImage;
DepthImageView div;
div.computeColorMap(300, 2000, 128);
div.convertToQImage(refQImage, aligner->correspondenceFinder()->referenceDepthImage());
div.convertToQImage(currQImage, aligner->correspondenceFinder()->currentDepthImage());
refScn->addPixmap((QPixmap::fromImage(refQImage)).scaled(QSize((int)refQImage.width()/(ng_scale*3), (int)(refQImage.height()/(ng_scale*3)))));
currScn->addPixmap((QPixmap::fromImage(currQImage)).scaled(QSize((int)currQImage.width()/(ng_scale*3), (int)(currQImage.height()/(ng_scale*3)))));
pwnGMW->graphicsView1_2d->show();
pwnGMW->graphicsView2_2d->show();
wasInitialGuess = false;
newCloudAdded = false;
*initialGuessViewer = 0;
*optimizeViewer = 0;
}
bool GraphGridMapper::convertLaserScans2Map(SlamGraph &graph, nav_msgs::OccupancyGrid &map, ros::Publisher &mapPub, Eigen::Isometry3d diff_transform)
{
std::string map_frame = "/map";
double diff_translation = diff_transform.translation().norm();
Eigen::AngleAxisd diff_angleaxis(diff_transform.linear());
double diff_rotation = fabs(diff_angleaxis.angle()) * 180 / M_PI;
ROS_DEBUG("map -> odom diff: %f m, %f deg", diff_translation, diff_rotation);
std::vector<std::string> nodes_to_add;
if (diff_translation <= 0.5 && diff_rotation < 5 && last_node_id_ != "") {
nodes_to_add = graph.getNodesAfter(last_node_id_);
} else {
// Clear occupancy grid.
clouds_.clear();
//setup map dimensions
nav_msgs::MapMetaData info;
cv::Point2d gridSize;
info.origin = getMapOrigin(graph, gridSize);
info.resolution = config_.resolution;
info.width = gridSize.x / info.resolution;
info.height = gridSize.y / info.resolution;
fake_grid_.info = info;
overlay_ = occupancy_grid_utils::createCloudOverlay(fake_grid_, map_frame, 0.1, 10, 1);
for (auto node_it = graph.nodeIterator(); node_it.first != node_it.second; node_it.first++) {
nodes_to_add.push_back(node_it.first->first);
}
}
if (nodes_to_add.size() > 0) {
last_node_id_ = nodes_to_add.back();
}
for (auto node_id : nodes_to_add) {
if (graph.existsNode(node_id)) {
const SlamNode &node = graph.node(node_id);
auto node_pose = affineTransformToPoseMsg(node.pose_);
occupancy_grid_utils::addKnownFreePoint(&overlay_, node_pose.position, 0.5);
// If there is a node off the grid, force clearing in the next iteration.
if (node.pose_.translation()(0) < fake_grid_.info.origin.position.x + config_.range_max ||
node.pose_.translation()(1) < fake_grid_.info.origin.position.y + config_.range_max ||
node.pose_.translation()(0) > fake_grid_.info.origin.position.x + fake_grid_.info.width * fake_grid_.info.resolution - config_.range_max ||
node.pose_.translation()(1) > fake_grid_.info.origin.position.y + fake_grid_.info.height * fake_grid_.info.resolution - config_.range_max) {
last_node_id_ = "";
}
}
}
for (auto node_id : nodes_to_add) {
if (graph.existsNode(node_id)) {
const SlamNode &node = graph.node(node_id);
// Search for laser scan.
int total_points = 0;
for (auto data : node.sensor_data_) {
if (data->type_ == graph_slam_msgs::SensorData::SENSOR_TYPE_LASERSCAN) {
boost::shared_ptr<LaserscanData> depth_data = boost::dynamic_pointer_cast<LaserscanData>(data);
sensor_msgs::LaserScan laser = depth_data->scan_;
laser.range_max = config_.range_max;
try
{
total_points += laser.ranges.size();
sensor_msgs::PointCloud fixed_frame_cloud;
laser_geometry::LaserProjection projector;
projector.projectLaser(laser,fixed_frame_cloud);
// Construct and save LocalizedCloud
CloudPtr loc_cloud(new occupancy_grid_utils::LocalizedCloud());
loc_cloud->cloud.points = fixed_frame_cloud.points;
loc_cloud->sensor_pose = affineTransformToPoseMsg(node.pose_ * depth_data->displacement_);
loc_cloud->header.frame_id = map_frame;
loc_cloud->header.stamp = laser.header.stamp;
clouds_.push_back(loc_cloud);
if (clouds_.size() > 1 && total_points > 0) {
occupancy_grid_utils::addCloud(&overlay_, clouds_.back());
}
}
catch (tf::LookupException& e)
{
ROS_WARN("lookup error");
return false;
}
catch (tf::ExtrapolationException)
{
ROS_ERROR("extrapolation exception");
return false;
}
catch (tf::ConnectivityException)
{
ROS_ERROR("connectivity exception");
return false;
}
}
}
}
}
//retrieve grid
nav_msgs::OccupancyGridConstPtr grid = occupancy_grid_utils::getGrid(overlay_);
map = *grid;
return true;
}
Vector6d transformCartesianLine(const Eigen::Isometry3d& t, const Vector6d& line){
Vector6d l;
l.head<3>() = t*line.head<3>();
l.tail<3>() = t.linear()*line.tail<3>();
return normalizeCartesianLine(l);
}