void SRBASolver::AddConstraint(int sourceId, int targetId, const karto::Pose2 &rDiff, const karto::Matrix3& rCovariance)
{
// Need to call create_kf2kf_edge here
srba_t::new_kf_observations_t list_obs;
srba_t::new_kf_observation_t obs_field;
obs_field.is_fixed = false; // "Landmarks" (relative poses) have unknown relative positions (i.e. treat them as unknowns to be estimated)
obs_field.is_unknown_with_init_val = false; // Ignored, since all observed "fake landmarks" already have an initialized value.
bool reverse_edge = false;
if(sourceId < targetId)
{
ROS_ERROR("REVERSE EDGE");
reverse_edge = true;
}
else
ROS_ERROR("$$$$$$$$$$$$$$$$NOT REVERSE EDGE");
karto::Matrix3 precisionMatrix = rCovariance.Inverse();
Eigen::Matrix<double,3,3> m;
m(0,0) = precisionMatrix(0,0);
m(0,1) = m(1,0) = precisionMatrix(0,1);
m(0,2) = m(2,0) = precisionMatrix(0,2);
m(1,1) = precisionMatrix(1,1);
m(1,2) = m(2,1) = precisionMatrix(1,2);
m(2,2) = precisionMatrix(2,2);
// if(reverse_edge)
// {
CPose2D pose(rDiff.GetX(), rDiff.GetY(), rDiff.GetHeading());
pose.inverse();
obs_field.obs.feat_id = sourceId; // Is this right??
obs_field.obs.obs_data.x = pose.x();
obs_field.obs.obs_data.y = pose.y();
obs_field.obs.obs_data.yaw = pose.phi();
// }
// else
/* {
obs_field.obs.feat_id = targetId; // Is this right??
obs_field.obs.obs_data.x = rDiff.GetX();
obs_field.obs.obs_data.y = rDiff.GetY();
obs_field.obs.obs_data.yaw = rDiff.GetHeading();
}*/
list_obs.push_back( obs_field );
std::vector<srba::TNewEdgeInfo> new_edge_ids;
//if(reverse_edge)
// {
rba_.add_observation(targetId, obs_field.obs, NULL, NULL );
// rba_.determine_kf2kf_edges_to_create(targetId,
// list_obs,
// new_edge_ids);
ROS_INFO("Created new edge from source: %d to target %d (%f, %f, %f)", sourceId, targetId, -rDiff.GetX(), -rDiff.GetY(), -rDiff.GetHeading());
//}
/* else
{
rba_.determine_kf2kf_edges_to_create(sourceId,
list_obs,
new_edge_ids);
rba_.add_observation(sourceId, obs_field.obs, NULL, NULL );
}*/
}
/**
* @brief SlamKarto::addScan
* @param laser LaserRangeFinder ref base
* @param scan 传感器数据 ref laser
* @param karto_pose odom_pose 回调
* @return
*/
bool
SlamKarto::addScan(karto::LaserRangeFinder* laser,
const sensor_msgs::LaserScan::ConstPtr& scan,
karto::Pose2& karto_pose)
{
if(!getOdomPose(karto_pose, scan->header.stamp)) // 将odom与base保持坐标系对齐． 返回 odom --> karto_pose
return false;
// Create a vector of doubles for karto
std::vector<kt_double> readings;
//readings 原始scan laser -> reading
if (lasers_inverted_[scan->header.frame_id])
{
for(std::vector<float>::const_reverse_iterator it = scan->ranges.rbegin();
it != scan->ranges.rend();
++it)
{
readings.push_back(*it); //laser range messages
}
}
else
{
for(std::vector<float>::const_iterator it = scan->ranges.begin();
it != scan->ranges.end();
++it)
{
readings.push_back(*it);
debugPrint_<<" "<< *it;
}
}
debugPrint_<<" " <<endl;
if(debug_flag_)
debugPrint_<<"reading.size() "<<readings.size()<<endl;
// create localized range scan 创建rangeScan
karto::LocalizedRangeScan* range_scan =
new karto::LocalizedRangeScan(laser->GetName(), readings);//
range_scan->SetOdometricPose(karto_pose);// Sets the odometric pose of this scan
range_scan->SetCorrectedPose(karto_pose);// Moves the scan by moving the robot pose to the given location.
if(debug_print_flag_)
debugPrint_<<"1.1 addScan-karto_pose-: "<<karto_pose.GetX()<<" "<<karto_pose.GetY()<<" "<< karto_pose.GetHeading() <<" "<<endl;
// Add the localized range scan to the mapper
bool processed; //range_scan contain reading and predict pose of robot
if((processed = mapper_->Process(range_scan))) // pose 为base相对odom的pose
{
//std::cout << "Pose: " << range_scan->GetOdometricPose() << " Corrected Pose: " << range_scan->GetCorrectedPose() << std::endl;
karto::Pose2 corrected_pose = range_scan->GetCorrectedPose();
if(debug_print_flag_)
debugPrint_<<"1.2 addScan-corrected_pose-: "<<corrected_pose.GetX()<<" "<<corrected_pose.GetY()<<" "<< corrected_pose.GetHeading() <<" "<<endl;
// Compute the map->odom transform
tf::Stamped<tf::Pose> odom_to_map;
try
{
tf_.transformPose(odom_frame_,tf::Stamped<tf::Pose> (tf::Transform( tf::createQuaternionFromRPY(0, 0, corrected_pose.GetHeading()),
tf::Vector3(corrected_pose.GetX(), corrected_pose.GetY(), 0.0)).inverse(),
scan->header.stamp, base_frame_),odom_to_map);
}
catch(tf::TransformException e)
{
ROS_ERROR("Transform from base_link to odom failed\n");
odom_to_map.setIdentity();
}
map_to_odom_mutex_.lock();
map_to_odom_ = tf::Transform(tf::Quaternion( odom_to_map.getRotation() ),
tf::Point( odom_to_map.getOrigin() ) ).inverse();
map_to_odom_mutex_.unlock();
//debugPrint_<< " x y z "<< odom_to_map.getOrigin().getX() <<" "<< odom_to_map.getOrigin().getY()<<" "<<odom_to_map.getOrigin().getZ()<<endl;
if(debug_print_flag_)
debugPrint_<<"1.3 addScan-odom_to_map: "<<odom_to_map.getOrigin().getX() <<" "<< odom_to_map.getOrigin().getY()<<" "<<endl;
// Add the localized range scan to the dataset (for memory management)
dataset_->Add(range_scan);
}
else
delete range_scan;
return processed;
}
