//==============================================================================
Eigen::Vector6d FreeJoint::getPositionDifferencesStatic(
const Eigen::Vector6d& _q2,
const Eigen::Vector6d& _q1) const
{
const Eigen::Isometry3d T1 = convertToTransform(_q1);
const Eigen::Isometry3d T2 = convertToTransform(_q2);
return convertToPositions(T1.inverse() * T2);
}
void RegApp::featuresReferenceHandler(const lcm::ReceiveBuffer* rbuf, const std::string& channel, const reg::features_t* msg){
if (registeration_mode_ != 1){ // ignore features if we havent just been asked to track to them
return;
}
cout << "\n\ngot reference features @ "<< msg->utime<<"\n";
// now find the corresponding image and pair it
bool feat_image_matched_ = false;
for(int i=0; i < image_queue_->size(); i++){
//cout << image_queue_->at(i).utime << " " << i << "\n";
if (msg->utime == image_queue_->at(i).utime){
cur_features_ = getFeaturesFromLCM(msg,cur_pose_);
cur_image_ = image_queue_->at(i);
cout << "image and features matched: " << i << " " << image_queue_->at(i).utime << " ("<< cur_features_.size()<<"f)\n";
feat_image_matched_ = true;
break;
}
}
if (!feat_image_matched_){
cout <<"didn't find a matching reference image - make image deque bigger\n";
exit(-1);
}
ref_features_ = cur_features_;
ref_image_ = cur_image_;
ref_pose_ = cur_pose_;
// set the reference image
cout << "Will now register to " << ref_features_.size() << " features\n";
// Send the pose we will try to register back to:
Isometry3dTime ref_poseT = Isometry3dTime(ref_image_.utime, ref_pose_);
pc_vis_->pose_to_lcm_from_list(70000, ref_poseT);
deque_to_cloud(laser_queue_, botframes_, accum_cloud);
Isometry3dTime null_poseT = Isometry3dTime(ref_image_.utime, Eigen::Isometry3d::Identity() );
pc_vis_->pose_to_lcm_from_list(70001, null_poseT);
pc_vis_->ptcld_to_lcm_from_list(70002, *accum_cloud, ref_image_.utime, ref_image_.utime);
std::stringstream ss2;
print_Isometry3d(ref_pose_, ss2);
std::cout << "Received refpose: " << ss2.str() << "\n";
Eigen::Isometry3f pose_f = ref_pose_.inverse().cast<float>();
Eigen::Quaternionf pose_quat(pose_f.rotation());
pcl::transformPointCloud (*accum_cloud, *accum_cloud,
pose_f.translation(), pose_quat); // !! modifies accum_cloud
pc_vis_->ptcld_to_lcm_from_list(70003, *accum_cloud, ref_image_.utime, ref_image_.utime);
registeration_mode_ = 2;
}
void foot_contact_classify::determineContactPoints(int64_t utime, Eigen::Isometry3d primary_foot, Eigen::Isometry3d secondary_foot){
// Currently Deprecated to remove PCL dependency
//////////////////////////////////////////////////////////
bool success =false;
// use primary and secondary foot to determine plane that CPs are one
Isometry3dTime null_pose = Isometry3dTime(utime, Eigen::Isometry3d::Identity() );
pc_vis_->pose_to_lcm_from_list(2001, null_pose);
// Determine moving contact points in stationary foot's sole frame:
// I define the sole frame as a frame directly below the foot frame on the sole
pronto::PointCloud* cp_moving(new pronto::PointCloud ());
Eigen::Isometry3d foot_to_foot = primary_foot.inverse() * secondary_foot * foot_to_sole_;
pc_vis_->transformPointCloud(*contact_points_, *cp_moving, Eigen::Affine3f ( foot_to_foot.cast<float>() ) );
pc_vis_->ptcld_to_lcm_from_list(2004, *cp_moving, utime, utime); // this is visualised relative to 0,0,0
if (cp_moving_prev_->points.size() != 4){
std::cout << "Previous contact points not four - we have a problem\n";
success = false;
}else{
int n_points_in_contact = 0;
for (size_t i=0; i < 4 ; i++){
pronto::Point cp = cp_moving->points[i];
pronto::Point cp_prev = cp_moving_prev_->points[i];
double raise = fabs(cp.z);
if ( raise < 0.02){
n_points_in_contact++;
//std::cout <<utime << " "<< raise << " " << (int)i << " cp in contact\n";
}else{
//std::cout <<utime << " "<< raise << " " << (int)i << " cp NOT in contact\n";
}
}
if (n_points_in_contact >0){
std::cout << utime << " " << n_points_in_contact << "\n";
}
success = true;
}
cp_moving_prev_ = cp_moving;
// determine the velocity of the SF CPs onto the PFCP plane
// infer the time to contact by differencing
// If the distance of the foot to the plane is less than a certain amount
// and the time to contact is less than a certain amount
// then contact is likely
}
void EdgeSE3Offset::initialEstimate(const OptimizableGraph::VertexSet& from_, OptimizableGraph::Vertex* /*to_*/) {
VertexSE3 *from = static_cast<VertexSE3*>(_vertices[0]);
VertexSE3 *to = static_cast<VertexSE3*>(_vertices[1]);
Eigen::Isometry3d virtualMeasurement = _cacheFrom->offsetParam()->offset() * measurement() * _cacheTo->offsetParam()->offset().inverse();
if (from_.count(from) > 0) {
to->setEstimate(from->estimate() * virtualMeasurement);
} else
from->setEstimate(to->estimate() * virtualMeasurement.inverse());
}
const Eigen::Matrix3d CMiniVisionToolbox::getEssential( const Eigen::Isometry3d& p_matTransformationFrom, const Eigen::Isometry3d& p_matTransformationTo )
{
//ds compute essential matrix: http://en.wikipedia.org/wiki/Essential_matrix TODO check math!
const Eigen::Isometry3d matTransformation( p_matTransformationTo.inverse( )*p_matTransformationFrom );
//const Eigen::Isometry3d matTransformation( p_matTransformationFrom.inverse( )*p_matTransformationTo );
//const Eigen::Isometry3d matTransformation( p_matTransformationTo*p_matTransformationFrom.inverse( ) );
const Eigen::Matrix3d matSkewTranslation( CMiniVisionToolbox::getSkew( matTransformation.translation( ) ) );
//ds compute essential matrix
return matTransformation.linear( )*matSkewTranslation;
}
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;
}
double BodyNode::TransformObjFunc::eval(Eigen::Map<const Eigen::VectorXd>& _x)
{
assert(mBodyNode->getParentJoint()->getNumGenCoords() == _x.size());
// Update forward kinematics information with _x
// We are just insterested in transformation of mBodyNode
mBodyNode->getParentJoint()->setConfigs(_x, true, false, false);
// Compute and return the geometric distance between body node transformation
// and target transformation
Eigen::Isometry3d bodyT = mBodyNode->getWorldTransform();
Eigen::Vector6d dist = math::logMap(bodyT.inverse() * mT);
return dist.dot(dist);
}
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();
}
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;
}
TEST(LIE_GROUP_OPERATORS, ADJOINT_MAPPINGS)
{
int numTest = 100;
// AdT(V) == T * V * InvT
for (int i = 0; i < numTest; ++i)
{
Eigen::Vector6d t = Eigen::Vector6d::Random();
Eigen::Isometry3d T = math::expMap(t);
Eigen::Vector6d V = Eigen::Vector6d::Random();
Eigen::Vector6d AdTV = AdT(T, V);
// Ad(T, V) = T * [V] * InvT
Eigen::Matrix4d T_V_InvT = T.matrix() * toMatrixForm(V) * T.inverse().matrix();
Eigen::Vector6d T_V_InvT_se3 = fromMatrixForm(T_V_InvT);
for (int j = 0; j < 6; ++j)
EXPECT_NEAR(AdTV(j), T_V_InvT_se3(j), LIE_GROUP_OPT_TOL);
// Ad(T, V) = [R 0; [p]R R] * V
Eigen::Matrix6d AdTMatrix = Eigen::Matrix6d::Zero();
AdTMatrix.topLeftCorner<3,3>() = T.linear();
AdTMatrix.bottomRightCorner<3,3>() = T.linear();
AdTMatrix.bottomLeftCorner<3,3>() = math::makeSkewSymmetric(T.translation()) * T.linear();
Eigen::Vector6d AdTMatrix_V = AdTMatrix * V;
for (int j = 0; j < 6; ++j)
EXPECT_NEAR(AdTV(j), AdTMatrix_V(j), LIE_GROUP_OPT_TOL);
}
// AdR == AdT([R 0; 0 1], V)
for (int i = 0; i < numTest; ++i)
{
Eigen::Vector6d t = Eigen::Vector6d::Random();
Eigen::Isometry3d T = math::expMap(t);
Eigen::Isometry3d R = Eigen::Isometry3d::Identity();
R = T.linear();
Eigen::Vector6d V = Eigen::Vector6d::Random();
Eigen::Vector6d AdTV = AdT(R, V);
Eigen::Vector6d AdRV = AdR(T, V);
for (int j = 0; j < 6; ++j)
EXPECT_NEAR(AdTV(j), AdRV(j), LIE_GROUP_OPT_TOL);
}
// AdTAngular == AdT(T, se3(w, 0))
for (int i = 0; i < numTest; ++i)
{
Eigen::Vector6d t = Eigen::Vector6d::Random();
Eigen::Isometry3d T = math::expMap(t);
Eigen::Vector3d w = Eigen::Vector3d::Random();
Eigen::Vector6d V = Eigen::Vector6d::Zero();
V.head<3>() = w;
Eigen::Vector6d AdTV = AdT(T, V);
Eigen::Vector6d AdTAng = AdTAngular(T, w);
for (int j = 0; j < 6; ++j)
EXPECT_NEAR(AdTV(j), AdTAng(j), LIE_GROUP_OPT_TOL);
}
// AdTLinear == AdT(T, se3(w, 0))
for (int i = 0; i < numTest; ++i)
{
Eigen::Vector6d t = Eigen::Vector6d::Random();
Eigen::Isometry3d T = math::expMap(t);
Eigen::Vector3d v = Eigen::Vector3d::Random();
Eigen::Vector6d V = Eigen::Vector6d::Zero();
V.tail<3>() = v;
Eigen::Vector6d AdTV = AdT(T, V);
Eigen::Vector6d AdTLin = AdTLinear(T, v);
for (int j = 0; j < 6; ++j)
EXPECT_NEAR(AdTV(j), AdTLin(j), LIE_GROUP_OPT_TOL);
}
// AdTJac
for (int i = 0; i < numTest; ++i)
{
Eigen::Vector6d t = Eigen::Vector6d::Random();
Eigen::Isometry3d T = math::expMap(t);
Eigen::Vector3d v = Eigen::Vector3d::Random();
Eigen::Vector6d V = Eigen::Vector6d::Zero();
V.tail<3>() = v;
Eigen::Vector6d AdTV = AdT(T, V);
Eigen::Vector6d AdTLin = AdTLinear(T, v);
for (int j = 0; j < 6; ++j)
EXPECT_NEAR(AdTV(j), AdTLin(j), LIE_GROUP_OPT_TOL);
}
// AdInvT
for (int i = 0; i < numTest; ++i)
{
Eigen::Vector6d t = Eigen::Vector6d::Random();
Eigen::Isometry3d T = math::expMap(t);
Eigen::Isometry3d InvT = T.inverse();
Eigen::Vector6d V = Eigen::Vector6d::Random();
Eigen::Vector6d Ad_InvT = AdT(InvT, V);
Eigen::Vector6d AdInv_T = AdInvT(T, V);
for (int j = 0; j < 6; ++j)
EXPECT_NEAR(Ad_InvT(j), AdInv_T(j), LIE_GROUP_OPT_TOL);
}
// AdInvRLinear
for (int i = 0; i < numTest; ++i)
{
Eigen::Vector6d t = Eigen::Vector6d::Random();
Eigen::Isometry3d T = math::expMap(t);
Eigen::Vector3d v = Eigen::Vector3d::Random();
Eigen::Vector6d V = Eigen::Vector6d::Zero();
V.tail<3>() = v;
Eigen::Isometry3d R = Eigen::Isometry3d::Identity();
R = T.linear();
Eigen::Vector6d AdT_ = AdT(R.inverse(), V);
Eigen::Vector6d AdInvRLinear_ = AdInvRLinear(T, v);
for (int j = 0; j < 6; ++j)
EXPECT_NEAR(AdT_(j), AdInvRLinear_(j), LIE_GROUP_OPT_TOL);
}
// dAdT
for (int i = 0; i < numTest; ++i)
{
Eigen::Vector6d t = Eigen::Vector6d::Random();
Eigen::Isometry3d T = math::expMap(t);
Eigen::Vector6d F = Eigen::Vector6d::Random();
Eigen::Vector6d dAdTF = dAdT(T, F);
// dAd(T, F) = [R 0; [p]R R]^T * F
Eigen::Matrix6d AdTMatrix = Eigen::Matrix6d::Zero();
AdTMatrix.topLeftCorner<3,3>() = T.linear();
AdTMatrix.bottomRightCorner<3,3>() = T.linear();
AdTMatrix.bottomLeftCorner<3,3>() = math::makeSkewSymmetric(T.translation()) * T.linear();
Eigen::Vector6d AdTTransMatrix_V = AdTMatrix.transpose() * F;
for (int j = 0; j < 6; ++j)
EXPECT_NEAR(dAdTF(j), AdTTransMatrix_V(j), LIE_GROUP_OPT_TOL);
}
// dAdInvT
for (int i = 0; i < numTest; ++i)
{
Eigen::Vector6d t = Eigen::Vector6d::Random();
Eigen::Isometry3d T = math::expMap(t);
Eigen::Isometry3d InvT = T.inverse();
Eigen::Vector6d F = Eigen::Vector6d::Random();
Eigen::Vector6d dAdInvT_F = dAdInvT(T, F);
//
Eigen::Vector6d dAd_InvTF = dAdT(InvT, F);
for (int j = 0; j < 6; ++j)
EXPECT_NEAR(dAdInvT_F(j), dAd_InvTF(j), LIE_GROUP_OPT_TOL);
// dAd(T, F) = [R 0; [p]R R]^T * F
Eigen::Matrix6d AdInvTMatrix = Eigen::Matrix6d::Zero();
AdInvTMatrix.topLeftCorner<3,3>() = InvT.linear();
AdInvTMatrix.bottomRightCorner<3,3>() = InvT.linear();
AdInvTMatrix.bottomLeftCorner<3,3>() = math::makeSkewSymmetric(InvT.translation()) * InvT.linear();
Eigen::Vector6d AdInvTTransMatrix_V = AdInvTMatrix.transpose() * F;
for (int j = 0; j < 6; ++j)
EXPECT_NEAR(dAdInvT_F(j), AdInvTTransMatrix_V(j), LIE_GROUP_OPT_TOL);
}
// dAdInvR
for (int i = 0; i < numTest; ++i)
{
Eigen::Vector6d t = Eigen::Vector6d::Random();
Eigen::Isometry3d T = math::expMap(t);
Eigen::Isometry3d InvT = T.inverse();
Eigen::Isometry3d InvR = Eigen::Isometry3d::Identity();
InvR = InvT.linear();
Eigen::Vector6d F = Eigen::Vector6d::Random();
Eigen::Vector6d dAdInvR_F = dAdInvR(T, F);
//
Eigen::Vector6d dAd_InvTF = dAdT(InvR, F);
for (int j = 0; j < 6; ++j)
EXPECT_NEAR(dAdInvR_F(j), dAd_InvTF(j), LIE_GROUP_OPT_TOL);
}
// ad
for (int i = 0; i < numTest; ++i)
{
Eigen::Vector6d V = Eigen::Vector6d::Random();
Eigen::Vector6d W = Eigen::Vector6d::Random();
Eigen::Vector6d ad_V_W = ad(V, W);
//
Eigen::Matrix6d adV_Matrix = Eigen::Matrix6d::Zero();
adV_Matrix.topLeftCorner<3,3>() = math::makeSkewSymmetric(V.head<3>());
adV_Matrix.bottomRightCorner<3,3>() = math::makeSkewSymmetric(V.head<3>());
adV_Matrix.bottomLeftCorner<3,3>() = math::makeSkewSymmetric(V.tail<3>());
Eigen::Vector6d adV_Matrix_W = adV_Matrix * W;
for (int j = 0; j < 6; ++j)
EXPECT_NEAR(ad_V_W(j), adV_Matrix_W(j), LIE_GROUP_OPT_TOL);
}
// dad
for (int i = 0; i < numTest; ++i)
{
Eigen::Vector6d V = Eigen::Vector6d::Random();
Eigen::Vector6d F = Eigen::Vector6d::Random();
Eigen::Vector6d dad_V_F = dad(V, F);
//
Eigen::Matrix6d dadV_Matrix = Eigen::Matrix6d::Zero();
dadV_Matrix.topLeftCorner<3,3>() = math::makeSkewSymmetric(V.head<3>());
dadV_Matrix.bottomRightCorner<3,3>() = math::makeSkewSymmetric(V.head<3>());
dadV_Matrix.bottomLeftCorner<3,3>() = math::makeSkewSymmetric(V.tail<3>());
Eigen::Vector6d dadV_Matrix_F= dadV_Matrix.transpose() * F;
for (int j = 0; j < 6; ++j)
EXPECT_NEAR(dad_V_F(j), dadV_Matrix_F(j), LIE_GROUP_OPT_TOL);
}
}
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;
}
int collideCylinderPlane(const double& cyl_rad, const double& half_height, const Eigen::Isometry3d& T0,
const Eigen::Vector3d& plane_normal, const Eigen::Isometry3d& T1,
std::vector<Contact>* result)
{
Eigen::Vector3d normal = T1.linear() * plane_normal;
Eigen::Vector3d Rx = T0.linear().rightCols(1);
Eigen::Vector3d Ry = normal - normal.dot(Rx) * Rx;
double mag = Ry.norm();
Ry.normalize();
if (mag < DART_COLLISION_EPS)
{
if (std::abs(Rx[2]) > 1.0 - DART_COLLISION_EPS)
Ry = Eigen::Vector3d::UnitX();
else
Ry = (Eigen::Vector3d(Rx[1], -Rx[0], 0.0)).normalized();
}
Eigen::Vector3d Rz = Rx.cross(Ry);
Eigen::Isometry3d T;
T.linear().col(0) = Rx;
T.linear().col(1) = Ry;
T.linear().col(2) = Rz;
T.translation() = T0.translation();
Eigen::Vector3d nn = T.linear().transpose() * normal;
Eigen::Vector3d pn = T.inverse() * T1.translation();
// four corners c0 = ( -h/2, -r ), c1 = ( +h/2, -r ), c2 = ( +h/2, +r ), c3 = ( -h/2, +r )
Eigen::Vector3d c[4] = {
Eigen::Vector3d(-half_height, -cyl_rad, 0.0),
Eigen::Vector3d(+half_height, -cyl_rad, 0.0),
Eigen::Vector3d(+half_height, +cyl_rad, 0.0),
Eigen::Vector3d(-half_height, +cyl_rad, 0.0) };
double depth[4] = { (pn - c[0]).dot(nn), (pn - c[1]).dot(nn), (pn - c[2]).dot(nn), (pn - c[3]).dot(nn) };
double penetration = -1.0;
int found = -1;
for (int i = 0; i < 4; i++)
{
if (depth[i] > penetration)
{
penetration = depth[i];
found = i;
}
}
Eigen::Vector3d point;
if (std::abs(depth[found] - depth[(found+1)%4]) < DART_COLLISION_EPS)
point = T * (0.5 * (c[found] + c[(found+1)%4]));
else if (std::abs(depth[found] - depth[(found+3)%4]) < DART_COLLISION_EPS)
point = T * (0.5 * (c[found] + c[(found+3)%4]));
else
point = T * c[found];
if (penetration > 0.0)
{
Contact contact;
contact.point = point;
contact.normal = normal;
contact.penetrationDepth = penetration;
result->push_back(contact);
return 1;
}
return 0;
}
int collideCylinderSphere(const double& cyl_rad, const double& half_height, const Eigen::Isometry3d& T0,
const double& sphere_rad, const Eigen::Isometry3d& T1,
std::vector<Contact>* result)
{
Eigen::Vector3d center = T0.inverse() * T1.translation();
double dist = sqrt(center[0] * center[0] + center[1] * center[1]);
if ( dist < cyl_rad && std::abs(center[2]) < half_height + sphere_rad )
{
Contact contact;
contact.penetrationDepth = 0.5 * (half_height + sphere_rad - math::sgn(center[2]) * center[2]);
contact.point = T0 * Eigen::Vector3d(center[0], center[1], half_height - contact.penetrationDepth);
contact.normal = T0.linear() * Eigen::Vector3d(0.0, 0.0, math::sgn(center[2]));
result->push_back(contact);
return 1;
}
else
{
double penetration = 0.5 * (cyl_rad + sphere_rad - dist);
if ( penetration > 0.0 )
{
if ( std::abs(center[2]) > half_height )
{
Eigen::Vector3d point = (Eigen::Vector3d(center[0], center[1], 0.0).normalized());
point *= cyl_rad;
point[2] = math::sgn(center[2]) * half_height;
Eigen::Vector3d normal = point - center;
penetration = sphere_rad - normal.norm();
normal = (T0.linear() * normal).normalized();
point = T0 * point;
if (penetration > 0.0)
{
Contact contact;
contact.point = point;
contact.normal = normal;
contact.penetrationDepth = penetration;
result->push_back(contact);
return 1;
}
}
else // if( center[2] >= -half_height && center[2] <= half_height )
{
Eigen::Vector3d point = (Eigen::Vector3d(center[0], center[1], 0.0)).normalized();
Eigen::Vector3d normal = -(T0.linear() * point);
point *= (cyl_rad - penetration);
point[2] = center[2];
point = T0 * point;
Contact contact;
contact.point = point;
contact.normal = normal;
contact.penetrationDepth = penetration;
result->push_back(contact);
return 1;
}
}
}
return 0;
}
//回环检测：在过去的帧中随机取_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;
}
int collideBoxSphere(CollisionObject* o1, CollisionObject* o2,
const Eigen::Vector3d& size0, const Eigen::Isometry3d& T0,
const double& r1, const Eigen::Isometry3d& T1,
CollisionResult& result)
{
Eigen::Vector3d halfSize = 0.5 * size0;
bool inside_box = true;
// clipping a center of the sphere to a boundary of the box
//Vec3 c0(&T0[9]);
Eigen::Vector3d c0 = T1.translation();
Eigen::Vector3d p = T0.inverse() * c0;
if (p[0] < -halfSize[0]) { p[0] = -halfSize[0]; inside_box = false; }
if (p[0] > halfSize[0]) { p[0] = halfSize[0]; inside_box = false; }
if (p[1] < -halfSize[1]) { p[1] = -halfSize[1]; inside_box = false; }
if (p[1] > halfSize[1]) { p[1] = halfSize[1]; inside_box = false; }
if (p[2] < -halfSize[2]) { p[2] = -halfSize[2]; inside_box = false; }
if (p[2] > halfSize[2]) { p[2] = halfSize[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 = halfSize[0] - std::abs(p[0]);
double tmin = halfSize[1] - std::abs(p[1]);
int idx = 0;
if ( tmin < min )
{
min = tmin;
idx = 1;
}
tmin = halfSize[2] - std::abs(p[2]);
if ( tmin < min )
{
min = tmin;
idx = 2;
}
//normal[idx] = (p[idx] > 0.0 ? 1.0 : -1.0);
normal[idx] = (p[idx] > 0.0 ? -1.0 : 1.0);
normal = T0.linear() * normal;
penetration = min + r1;
Contact contact;
contact.collisionObject1 = o1;
contact.collisionObject2 = o2;
contact.point = c0;
contact.normal = normal;
contact.penetrationDepth = penetration;
result.addContact(contact);
return 1;
}
Eigen::Vector3d contactpt = T0 * p;
//normal = c0 - contactpt;
normal = contactpt - c0;
double mag = normal.norm();
penetration = r1 - mag;
if (penetration < 0.0)
{
return 0;
}
if (mag > DART_COLLISION_EPS)
{
normal *= (1.0/mag);
Contact contact;
contact.collisionObject1 = o1;
contact.collisionObject2 = o2;
contact.point = contactpt;
contact.normal = normal;
contact.penetrationDepth = penetration;
result.addContact(contact);
}
else
{
double min = halfSize[0] - std::abs(p[0]);
double tmin = halfSize[1] - std::abs(p[1]);
int idx = 0;
if ( tmin < min )
{
min = tmin;
idx = 1;
}
tmin = halfSize[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[idx] = (p[idx] > 0.0 ? -1.0 : 1.0);
normal = T0.linear() * normal;
Contact contact;
contact.collisionObject1 = o1;
contact.collisionObject2 = o2;
contact.point = contactpt;
contact.normal = normal;
contact.penetrationDepth = penetration;
result.addContact(contact);
}
return 1;
}
void writeQueue() {
SensorData* data;
std::ofstream ofG2O(&filename[0]);
geometry_msgs::TransformStamped msg;
int num = 0;
// this is the vertex where we are packing the data
g2o::VertexSE3* activeVertex = 0;
// this is the timestamp of the first measurement added to the vertex
double activeVertexTime=0;
// this is the previous vertex
g2o::VertexSE3* previousVertex = 0;
// this is the timestamp of the first measurement added to the previous vertex
double previousVertexTime=0;
// the last position of the robot (not of the vertex, needed to measure the distances)
Eigen::Isometry3d lastRobotPose;
// set of sensors we packed in the current data.
// We do not want to put 10 camera images of the same camera in the same vertex.
std::set<Sensor*> addedSensors;
Eigen::Vector2d distances(0.,0);
while (true)
{
if(! _queue.empty())
{
data = (SensorData*)_queue.front();
double timeNow = _queue.lastElementTime();
conditionalPrint(annoyingLevel) << "size=" << _queue.size() << " lastTime=" << FIXED(timeNow) << endl;
if (timeNow - data->timeStamp()> initialDelay)
{ // we have enough stuff in the queue
_queue.pop_front();
if (! nptr->ok())
continue;
tf::StampedTransform transform;
bool we_got_transf = false;
try
{
ros::Time timeStamp;
// Get transformation
(*tfListener).lookupTransform("/odom", "/base_link", timeStamp.fromSec(data->timeStamp()), transform);
we_got_transf = true;
}
catch (tf::TransformException & ex)
{
ROS_ERROR("%s", ex.what());
}
if (! we_got_transf)
continue;
Eigen::Isometry3d currentRobotPose = fromStampedTransform(transform);
double currentDataTime = data->timeStamp();
distances += isometry2distance(lastRobotPose.inverse()*currentRobotPose);
double passedTime = currentDataTime-previousVertexTime;
lastRobotPose = currentRobotPose;
conditionalPrint(annoyingLevel) << "distances: " << distances[0] << " " << distances[1] << " " << passedTime << endl;
if (distances[0] < minDistances[0] &&
distances[1] < minDistances[1] &&
passedTime < minTime){
conditionalPrint(annoyingLevel) << "reject: (time/distance)" << endl;
// SKIP THE FRAME
delete data;
data = 0;
continue;
}
if (!activeVertex) {
activeVertex = new g2o::VertexSE3();
activeVertex->setId(num);
activeVertex->setEstimate(fromStampedTransform(transform));
activeVertexTime = currentDataTime;
}
Sensor* sensor = data->sensor();
assert (sensor && "!");
// check if we already packed the data for this kind of sensor
if (addedSensors.count(sensor)){
conditionalPrint(annoyingLevel) << "reject: (sensor) "<< endl;
delete data;
} else {
addedSensors.insert(sensor);
Parameter* parameter = sensor->parameter();
assert (parameter && "!@#");
//data->writeOut(filename);
if (! graph->parameters().getParameter(parameter->id())){
graph->parameters().addParameter(parameter);
graph->saveParameter(ofG2O, parameter);
}
activeVertex->addUserData(data);
data->setDataContainer(activeVertex);
}
// detach the data from the thing
data = 0;
if (currentDataTime - activeVertexTime > vertexTimeWindow) {
conditionalPrint(annoyingLevel) << "flush" << endl;
graph->addVertex(activeVertex);
graph->saveVertex(ofG2O, activeVertex);
if (previousVertex) {
EdgeSE3* e = new EdgeSE3();
e->setVertex(0, previousVertex);
e->setVertex(1, activeVertex);
e->setMeasurementFromState();
Eigen::Matrix<double, 6,6> m;
m.setIdentity();
e->setInformation(m);
graph->addEdge(e);
graph->saveEdge(ofG2O, e);
// JACP: do not do the remove, scan the data list and do a release() of the images which are big. The rest can stay in memory
g2o::HyperGraph::Data* d = previousVertex->userData();
while (d) {
RGBDData* rgbd = dynamic_cast<RGBDData*> (d);
if (rgbd)
rgbd->release();
d = d->next();
}
vertecesQueue.push_back(previousVertex);
}
previousVertex = activeVertex;
previousVertexTime = activeVertexTime;
addedSensors.clear();
activeVertex = 0;
distances.setZero();
num++;
conditionalPrint(defaultLevel) << ".";
}
}
}
usleep(20e3); // queue is emp-ty
}
}
Eigen::Matrix4d ViewerEye::getView() const {
Eigen::Isometry3d transformedPose = getPoseIsometry();
return transformedPose.inverse().matrix();
}
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 );
}
}
const std::shared_ptr< std::vector< const CMeasurementLandmark* > > CMockedMatcherEpipolar::getVisibleLandmarks( const uint64_t p_uFrame,
cv::Mat& p_matDisplayLEFT,
cv::Mat& p_matDisplayRIGHT,
const cv::Mat& p_matImageLEFT,
const cv::Mat& p_matImageRIGHT,
const Eigen::Isometry3d& p_matTransformationLEFTtoWORLD,
cv::Mat& p_matDisplayTrajectory,
const double& p_dMotionScaling )
{
//ds detected landmarks at this position
std::shared_ptr< std::vector< const CMeasurementLandmark* > > vecVisibleLandmarks( std::make_shared< std::vector< const CMeasurementLandmark* > >( ) );
//ds precompute inverse once
const Eigen::Vector3d vecTranslation( p_matTransformationLEFTtoWORLD.translation( ) );
const Eigen::Isometry3d matTransformationWORLDtoLEFT( p_matTransformationLEFTtoWORLD.inverse( ) );
const MatrixProjection matProjectionWORLDtoLEFT( m_pCameraLEFT->m_matProjection*matTransformationWORLDtoLEFT.matrix( ) );
//ds current position
const cv::Point2d ptPositionXY( vecTranslation.x( ), vecTranslation.y( ) );
//ds get currently visible landmarks
const std::map< UIDLandmark, CMockedDetection > mapVisibleLandmarks( m_pCameraSTEREO->getDetectedLandmarks( ptPositionXY, matTransformationWORLDtoLEFT, p_matDisplayTrajectory ) );
//ds new active measurement points after this matching
std::vector< CDetectionPoint > vecMeasurementPointsActive;
//ds initial translation
const CPoint3DWORLD vecTranslationEstimate( p_matTransformationLEFTtoWORLD.translation( ) );
//ds vectors for pose solver
gtools::Vector3dVector vecLandmarksWORLD;
gtools::Vector2dVector vecImagePointsLEFT;
gtools::Vector2dVector vecImagePointsRIGHT;
//ds active measurements
for( const CDetectionPoint cMeasurementPoint: m_vecDetectionPointsActive )
{
//ds visible (=in this image detected) active (=not detected in this image but failed detections below threshold)
std::vector< const CMeasurementLandmark* > vecVisibleLandmarksPerMeasurementPoint;
std::shared_ptr< std::vector< CLandmark* > >vecActiveLandmarksPerMeasurementPoint( std::make_shared< std::vector< CLandmark* > >( ) );
//ds loop over the points for the current scan
for( CLandmark* pLandmarkReference: *cMeasurementPoint.vecLandmarks )
{
//ds projection from triangulation to estimate epipolar line drawing TODO: remove cast
const cv::Point2d ptProjection( m_pCameraLEFT->getProjection( static_cast< CPoint3DWORLD >( matTransformationWORLDtoLEFT*pLandmarkReference->vecPointXYZOptimized ) ) );
//ds draw last position
cv::circle( p_matDisplayLEFT, pLandmarkReference->getLastDetectionLEFT( ), 2, CColorCodeBGR( 255, 0, 0 ), -1 );
try
{
//ds look for the detection of this landmark (HACK: set mocked landmark id into keypoint size field to avoid another landmark class)
const CMockedDetection cDetection( mapVisibleLandmarks.at( static_cast< UIDLandmark >( pLandmarkReference->dKeyPointSize ) ) );
//ds triangulate point
const CPoint3DCAMERA vecPointTriangulatedLEFT( CMiniVisionToolbox::getPointStereoLinearTriangulationSVDLS( cDetection.ptUVLEFT,
cDetection.ptUVRIGHT,
m_pCameraLEFT->m_matProjection,
m_pCameraRIGHT->m_matProjection ) );
//ds update landmark
pLandmarkReference->bIsCurrentlyVisible = true;
pLandmarkReference->matDescriptorLASTLEFT = CDescriptor( );
pLandmarkReference->matDescriptorLASTRIGHT = CDescriptor( );
pLandmarkReference->uFailedSubsequentTrackings = 0;
pLandmarkReference->addMeasurement( p_uFrame, cDetection.ptUVLEFT, cDetection.ptUVRIGHT, vecPointTriangulatedLEFT, static_cast< CPoint3DWORLD >( p_matTransformationLEFTtoWORLD*vecPointTriangulatedLEFT ), matTransformationWORLDtoLEFT.translation( ), Eigen::Vector3d( 0.0, 0.0, 0.0 ), matProjectionWORLDtoLEFT, CDescriptor( ) );
//ds register measurement
vecVisibleLandmarksPerMeasurementPoint.push_back( pLandmarkReference->getLastMeasurement( ) );
//ds store elements for optimization
vecLandmarksWORLD.push_back( pLandmarkReference->vecPointXYZOptimized );
vecImagePointsLEFT.push_back( CWrapperOpenCV::fromCVVector( cDetection.ptUVLEFT ) );
vecImagePointsRIGHT.push_back( CWrapperOpenCV::fromCVVector( cDetection.ptUVRIGHT ) );
//ds new positions
cv::circle( p_matDisplayLEFT, pLandmarkReference->getLastDetectionLEFT( ), 2, CColorCodeBGR( 0, 255, 0 ), -1 );
char chBufferMiniInfo[20];
std::snprintf( chBufferMiniInfo, 20, "%lu(%u|%5.2f)", pLandmarkReference->uID, pLandmarkReference->uOptimizationsSuccessful, pLandmarkReference->dCurrentAverageSquaredError );
cv::putText( p_matDisplayLEFT, chBufferMiniInfo, cv::Point2d( cDetection.ptUVLEFT.x+10, cDetection.ptUVLEFT.y+10 ), cv::FONT_HERSHEY_PLAIN, 0.8, CColorCodeBGR( 0, 0, 255 ) );
//ds draw reprojection of triangulation
cv::circle( p_matDisplayRIGHT, cDetection.ptUVRIGHT, 2, CColorCodeBGR( 0, 255, 0 ), -1 );
cv::circle( p_matDisplayLEFT, ptProjection, 10, CColorCodeBGR( 0, 0, 255 ), 1 );
}
catch( const std::out_of_range& p_eException )
{
//ds draw last position
cv::circle( p_matDisplayLEFT, pLandmarkReference->getLastDetectionLEFT( ), 2, CColorCodeBGR( 255, 0, 0 ), -1 );
++pLandmarkReference->uFailedSubsequentTrackings;
//std::printf( "<CMockedMatcherEpipolar>(getVisibleLandmarksEssential) landmark [%lu] caught exception: %s\n", pLandmarkReference->uID, p_eException.what( ) );
}
//ds check activity
if( m_uMaximumFailedSubsequentTrackingsPerLandmark > pLandmarkReference->uFailedSubsequentTrackings )
{
vecActiveLandmarksPerMeasurementPoint->push_back( pLandmarkReference );
}
else
{
std::printf( "<CMockedMatcherEpipolar>(getVisibleLandmarksEssential) landmark [%lu] dropped\n", pLandmarkReference->uID );
}
}
//ds log
CLogger::CLogDetectionEpipolar::addEntry( p_uFrame, cMeasurementPoint.uID, cMeasurementPoint.vecLandmarks->size( ), vecActiveLandmarksPerMeasurementPoint->size( ), vecVisibleLandmarksPerMeasurementPoint.size( ) );
//ds check if we can keep the measurement point
if( !vecActiveLandmarksPerMeasurementPoint->empty( ) )
{
//ds register the measurement point and its visible landmarks anew
vecMeasurementPointsActive.push_back( CDetectionPoint( cMeasurementPoint.uID, cMeasurementPoint.matTransformationLEFTtoWORLD, vecActiveLandmarksPerMeasurementPoint ) );
//ds combine visible landmarks
vecVisibleLandmarks->insert( vecVisibleLandmarks->end( ), vecVisibleLandmarksPerMeasurementPoint.begin( ), vecVisibleLandmarksPerMeasurementPoint.end( ) );
}
else
{
std::printf( "<CMockedMatcherEpipolar>(getVisibleLandmarksEssential) erased detection point\n" );
}
}
//ds check if we have a sufficient number of points to optimize
if( m_uMinimumPointsForPoseOptimization < vecLandmarksWORLD.size( ) )
{
//ds feed the solver with the 3D points (in camera frame)
m_cSolverPoseSTEREO.model_points = vecLandmarksWORLD;
//ds feed the solver with the 2D points
m_cSolverPoseSTEREO.vecProjectionsUVLEFT = vecImagePointsLEFT;
m_cSolverPoseSTEREO.vecProjectionsUVRIGHT = vecImagePointsRIGHT;
//ds initial guess of the transformation
m_cSolverPoseSTEREO.T = matTransformationWORLDtoLEFT;
//ds initialize solver
m_cSolverPoseSTEREO.init( );
//ds convergence
double dErrorPrevious( 0.0 );
//ds run KLS
const uint8_t uMaxIterations = 10;
for( uint8_t uIteration = 0; uIteration < uMaxIterations; ++uIteration )
{
//ds run optimization
const double dErrorSolverPoseCurrent = m_cSolverPoseSTEREO.oneRound( );
const uint32_t uInliersCurrent = m_cSolverPoseSTEREO.uNumberOfInliers;
CLogger::CLogOptimizationOdometry::addEntryIteration( p_uFrame, uIteration, vecLandmarksWORLD.size( ), uInliersCurrent, m_cSolverPoseSTEREO.uNumberOfReprojections, dErrorSolverPoseCurrent );
//ds check convergence (triggers another last loop)
if( m_dConvergenceDeltaPoseOptimization > std::fabs( dErrorPrevious-dErrorSolverPoseCurrent ) )
{
//ds if we have a sufficient number of inliers
if( m_uMinimumInliersForPoseOptimization < uInliersCurrent )
{
//ds the last round is run with only inliers
const double dErrorSolverPoseCurrentIO = m_cSolverPoseSTEREO.oneRoundInliersOnly( );
//ds log the error evolution
CLogger::CLogOptimizationOdometry::addEntryInliers( p_uFrame, vecLandmarksWORLD.size( ), m_cSolverPoseSTEREO.uNumberOfInliers, m_cSolverPoseSTEREO.uNumberOfReprojections, dErrorSolverPoseCurrentIO );
}
else
{
std::printf( "<CMockedMatcherEpipolar>(getVisibleLandmarksEssential) unable to run loop on inliers only (%u not sufficient)\n", uInliersCurrent );
}
break;
}
//ds save error
dErrorPrevious = dErrorSolverPoseCurrent;
}
const Eigen::Isometry3d matTransformationLEFTtoWORLDCorrected( m_cSolverPoseSTEREO.T.inverse( ) );
//ds qualitiy information
const double dDeltaOptimization = ( matTransformationLEFTtoWORLDCorrected.translation( )-vecTranslationEstimate ).squaredNorm( );
const double dOptimizationCovariance = dDeltaOptimization/p_dMotionScaling;
//ds log resulting trajectory and delta to initial
CLogger::CLogOptimizationOdometry::addEntryResult( matTransformationLEFTtoWORLDCorrected.translation( ), dDeltaOptimization, dOptimizationCovariance );
const cv::Point2d ptPositionXY( matTransformationLEFTtoWORLDCorrected.translation( ).x( ), matTransformationLEFTtoWORLDCorrected.translation( ).y( ) );
cv::circle( p_matDisplayTrajectory, cv::Point2d( 180+ptPositionXY.x*10, 360-ptPositionXY.y*10 ), 2, CColorCodeBGR( 0, 0, 255 ), -1 );
}
else
{
std::printf( "<CMockedMatcherEpipolar>(getVisibleLandmarksEssential) unable to optimize pose (%lu points)\n", vecLandmarksWORLD.size( ) );
}
//ds update active measurement points
m_vecDetectionPointsActive.swap( vecMeasurementPointsActive );
//ds return active landmarks
return vecVisibleLandmarks;
}