Skip to content

Commit

Permalink
feat(accel_map_calibrator): publish estimated covariance (#2551)
Browse files Browse the repository at this point in the history 
* add calcuration of data covariance for 4cell-calibration.

* add publisher for offset_covariance.

* fix publisher of offset_covariance.

* refactor code

Signed-off-by: takahoribe <horibe.takamasa@gmail.com>

* change publisher variance to standard deviation.

* fix error

Signed-off-by: Takamasa Horibe <horibe.takamasa@gmail.com>

---------

Signed-off-by: takahoribe <horibe.takamasa@gmail.com>
Signed-off-by: Takamasa Horibe <horibe.takamasa@gmail.com>
Co-authored-by: takahoribe <horibe.takamasa@gmail.com>
  • Loading branch information
@td12321 @TakaHoribe
td12321 and TakaHoribe authored Jan 28, 2023
1 parent 097228f commit bac20ee
Show file tree
Hide file tree
Showing 2 changed files with 137 additions and 5 deletions.
17 changes: 17 additions & 0 deletions ...ake_map_calibrator/include/accel_brake_map_calibrator/accel_brake_map_calibrator_node.hpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -79,6 +79,7 @@ class AccelBrakeMapCalibrator : public rclcpp::Node
rclcpp::Publisher<nav_msgs::msg::OccupancyGrid>::SharedPtr update_map_occ_pub_;
rclcpp::Publisher<std_msgs::msg::Float32MultiArray>::SharedPtr original_map_raw_pub_;
rclcpp::Publisher<std_msgs::msg::Float32MultiArray>::SharedPtr update_map_raw_pub_;
rclcpp::Publisher<std_msgs::msg::Float32MultiArray>::SharedPtr offset_covariance_pub_;
rclcpp::Publisher<tier4_debug_msgs::msg::Float32MultiArrayStamped>::SharedPtr debug_pub_;
rclcpp::Publisher<nav_msgs::msg::OccupancyGrid>::SharedPtr data_count_pub_;
rclcpp::Publisher<nav_msgs::msg::OccupancyGrid>::SharedPtr data_count_with_self_pose_pub_;
Expand Down Expand Up @@ -184,6 +185,8 @@ class AccelBrakeMapCalibrator : public rclcpp::Node
std::vector<std::vector<double>> brake_map_value_;
std::vector<std::vector<double>> update_accel_map_value_;
std::vector<std::vector<double>> update_brake_map_value_;
std::vector<std::vector<double>> accel_offset_covariance_value_;
std::vector<std::vector<double>> brake_offset_covariance_value_;
std::vector<std::vector<std::vector<double>>> map_value_data_;
std::vector<double> accel_vel_index_;
std::vector<double> brake_vel_index_;
Expand Down Expand Up @@ -285,6 +288,17 @@ class AccelBrakeMapCalibrator : public rclcpp::Node
bool isTimeout(const builtin_interfaces::msg::Time & stamp, const double timeout_sec);
bool isTimeout(const DataStampedPtr & data_stamped, const double timeout_sec);

/* for covariance calculation */
// mean value on each cell (counting methoud depends on the update algorithm)
Eigen::MatrixXd accel_data_mean_mat_;
Eigen::MatrixXd brake_data_mean_mat_;
// calculated vairiance on each cell
Eigen::MatrixXd accel_data_covariance_mat_;
Eigen::MatrixXd brake_data_covariance_mat_;
// number of data on each cell (counting methoud depends on the update algorithm)
Eigen::MatrixXd accel_data_num_;
Eigen::MatrixXd brake_data_num_;

nav_msgs::msg::OccupancyGrid getOccMsg(
const std::string frame_id, const double height, const double width, const double resolution,
const std::vector<int8_t> & map_value);
Expand All @@ -296,6 +310,9 @@ class AccelBrakeMapCalibrator : public rclcpp::Node
void publishMap(
const std::vector<std::vector<double>> accel_map_value,
const std::vector<std::vector<double>> brake_map_value, const std::string publish_type);
void publishOffsetCovMap(
const std::vector<std::vector<double>> accel_map_value,
const std::vector<std::vector<double>> brake_map_value);
void publishCountMap();
void publishIndex();
bool writeMapToCSV(
Expand Down
125 changes: 120 additions & 5 deletions ...l_brake_map_calibrator/accel_brake_map_calibrator/src/accel_brake_map_calibrator_node.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -144,6 +144,14 @@ AccelBrakeMapCalibrator::AccelBrakeMapCalibrator(const rclcpp::NodeOptions & nod
for (auto & m : update_brake_map_value_) {
m.resize(brake_map_value_.at(0).size());
}
accel_offset_covariance_value_.resize((accel_map_value_.size()));
brake_offset_covariance_value_.resize((brake_map_value_.size()));
for (auto & m : accel_offset_covariance_value_) {
m.resize(accel_map_value_.at(0).size(), covariance_);
}
for (auto & m : brake_offset_covariance_value_) {
m.resize(brake_map_value_.at(0).size(), covariance_);
}
map_value_data_.resize(accel_map_value_.size() + brake_map_value_.size() - 1);
for (auto & m : map_value_data_) {
m.resize(accel_map_value_.at(0).size());
Expand All @@ -152,6 +160,19 @@ AccelBrakeMapCalibrator::AccelBrakeMapCalibrator(const rclcpp::NodeOptions & nod
std::copy(accel_map_value_.begin(), accel_map_value_.end(), update_accel_map_value_.begin());
std::copy(brake_map_value_.begin(), brake_map_value_.end(), update_brake_map_value_.begin());

// inialize matrix for covariance calculation
{
const auto genConstMat = [](const raw_vehicle_cmd_converter::Map & map, const auto val) {
return Eigen::MatrixXd::Constant(map.size(), map.at(0).size(), val);
};
accel_data_mean_mat_ = genConstMat(accel_map_value_, map_offset_);
brake_data_mean_mat_ = genConstMat(brake_map_value_, map_offset_);
accel_data_covariance_mat_ = genConstMat(accel_map_value_, covariance_);
brake_data_covariance_mat_ = genConstMat(brake_map_value_, covariance_);
accel_data_num_ = genConstMat(accel_map_value_, 1);
brake_data_num_ = genConstMat(brake_map_value_, 1);
}

// publisher
update_suggest_pub_ =
create_publisher<std_msgs::msg::Bool>("~/output/update_suggest", durable_qos);
Expand Down Expand Up @@ -181,6 +202,8 @@ AccelBrakeMapCalibrator::AccelBrakeMapCalibrator(const rclcpp::NodeOptions & nod
"/accel_brake_map_calibrator/output/updated_map_error", durable_qos);
map_error_ratio_pub_ = create_publisher<tier4_debug_msgs::msg::Float32Stamped>(
"/accel_brake_map_calibrator/output/map_error_ratio", durable_qos);
offset_covariance_pub_ = create_publisher<std_msgs::msg::Float32MultiArray>(
"/accel_brake_map_calibrator/debug/offset_covariance", durable_qos);

// subscriber
using std::placeholders::_1;
Expand Down Expand Up @@ -320,6 +343,7 @@ void AccelBrakeMapCalibrator::timerCallback()
/* publish map & debug_values*/
publishMap(accel_map_value_, brake_map_value_, "original");
publishMap(update_accel_map_value_, update_brake_map_value_, "update");
publishOffsetCovMap(accel_offset_covariance_value_, brake_offset_covariance_value_);
publishCountMap();
publishIndex();
publishUpdateSuggestFlag();
Expand Down Expand Up @@ -846,6 +870,8 @@ bool AccelBrakeMapCalibrator::updateFourCellAroundOffset(
accel_map_value_.at(0).size() - 1, Eigen::MatrixXd::Identity(4, 4) * covariance_));

auto & update_map_value = accel_mode ? update_accel_map_value_ : update_brake_map_value_;
auto & offset_covariance_value =
accel_mode ? accel_offset_covariance_value_ : brake_offset_covariance_value_;
const auto & map_value = accel_mode ? accel_map_value_ : brake_map_value_;
const auto & pedal_index = accel_mode ? accel_pedal_index : brake_pedal_index;
const auto & pedal_index_ = accel_mode ? accel_pedal_index_ : brake_pedal_index_;
Expand All @@ -855,6 +881,9 @@ bool AccelBrakeMapCalibrator::updateFourCellAroundOffset(
accel_mode ? delayed_accel_pedal_ptr_->data : delayed_brake_pedal_ptr_->data;
auto & map_offset_vec = accel_mode ? accel_map_offset_vec_ : brake_map_offset_vec_;
auto & covariance_mat = accel_mode ? accel_covariance_mat_ : brake_covariance_mat_;
auto & data_mean_mat = accel_mode ? accel_data_mean_mat_ : brake_data_mean_mat_;
auto & data_covariance_mat = accel_mode ? accel_data_covariance_mat_ : brake_data_covariance_mat_;
auto & data_weighted_num = accel_mode ? accel_data_num_ : brake_data_num_;

const double zll = update_map_value.at(pedal_index + 0).at(vel_index + 0);
const double zhl = update_map_value.at(pedal_index + 1).at(vel_index + 0);
Expand All @@ -874,6 +903,23 @@ bool AccelBrakeMapCalibrator::updateFourCellAroundOffset(
Eigen::Vector4d theta(4);
theta << zll, zhl, zlh, zhh;

Eigen::Vector4d weighted_sum(4);
weighted_sum << data_weighted_num(pedal_index + 0, vel_index + 0),
data_weighted_num(pedal_index + 1, vel_index + 0),
data_weighted_num(pedal_index + 0, vel_index + 1),
data_weighted_num(pedal_index + 1, vel_index + 1);

Eigen::Vector4d sigma(4);
sigma << data_covariance_mat(pedal_index + 0, vel_index + 0),
data_covariance_mat(pedal_index + 1, vel_index + 0),
data_covariance_mat(pedal_index + 0, vel_index + 1),
data_covariance_mat(pedal_index + 1, vel_index + 1);

Eigen::Vector4d mean(4);
mean << data_mean_mat(pedal_index + 0, vel_index + 0),
data_mean_mat(pedal_index + 1, vel_index + 0), data_mean_mat(pedal_index + 0, vel_index + 1),
data_mean_mat(pedal_index + 1, vel_index + 1);

const int vel_idx_l = vel_index + 0;
const int vel_idx_h = vel_index + 1;
const int ped_idx_l = pedal_index + 0;
Expand All @@ -885,6 +931,8 @@ bool AccelBrakeMapCalibrator::updateFourCellAroundOffset(
map_offset(2) = map_offset_vec.at(ped_idx_l).at(vel_idx_h);
map_offset(3) = map_offset_vec.at(ped_idx_h).at(vel_idx_h);

Eigen::VectorXd updated_map_offset(4);

Eigen::MatrixXd covariance = covariance_mat.at(ped_idx_l).at(vel_idx_l);

/* calculate adaptive map offset */
Expand All @@ -899,13 +947,38 @@ bool AccelBrakeMapCalibrator::updateFourCellAroundOffset(
double eta_hat = phiT * theta;

const double error_map_offset = measured_acc - eta_hat;
map_offset = map_offset + G * error_map_offset;
updated_map_offset = map_offset + G * error_map_offset;

for (int i = 0; i < 4; i++) {
const double pre_mean = mean(i);
mean(i) = (weighted_sum(i) * pre_mean + error_map_offset) / (weighted_sum(i) + 1);
sigma(i) =
(weighted_sum(i) * (sigma(i) + pre_mean * pre_mean) + error_map_offset * error_map_offset) /
(weighted_sum(i) + 1) -
mean(i) * mean(i);
weighted_sum(i) = weighted_sum(i) + 1;
}

/* input calculated result and update map */
map_offset_vec.at(ped_idx_l).at(vel_idx_l) = map_offset(0);
map_offset_vec.at(ped_idx_h).at(vel_idx_l) = map_offset(1);
map_offset_vec.at(ped_idx_l).at(vel_idx_h) = map_offset(2);
map_offset_vec.at(ped_idx_h).at(vel_idx_h) = map_offset(3);
map_offset_vec.at(ped_idx_l).at(vel_idx_l) = updated_map_offset(0);
map_offset_vec.at(ped_idx_h).at(vel_idx_l) = updated_map_offset(1);
map_offset_vec.at(ped_idx_l).at(vel_idx_h) = updated_map_offset(2);
map_offset_vec.at(ped_idx_h).at(vel_idx_h) = updated_map_offset(3);

data_covariance_mat(ped_idx_l, vel_idx_l) = sigma(0);
data_covariance_mat(ped_idx_h, vel_idx_l) = sigma(1);
data_covariance_mat(ped_idx_l, vel_idx_h) = sigma(2);
data_covariance_mat(ped_idx_h, vel_idx_h) = sigma(3);

data_weighted_num(ped_idx_l, vel_idx_l) = weighted_sum(0);
data_weighted_num(ped_idx_h, vel_idx_l) = weighted_sum(1);
data_weighted_num(ped_idx_l, vel_idx_h) = weighted_sum(2);
data_weighted_num(ped_idx_h, vel_idx_h) = weighted_sum(3);

data_mean_mat(ped_idx_l, vel_idx_l) = mean(0);
data_mean_mat(ped_idx_h, vel_idx_l) = mean(1);
data_mean_mat(ped_idx_l, vel_idx_h) = mean(2);
data_mean_mat(ped_idx_h, vel_idx_h) = mean(3);

covariance_mat.at(ped_idx_l).at(vel_idx_l) = covariance;

Expand All @@ -918,6 +991,11 @@ bool AccelBrakeMapCalibrator::updateFourCellAroundOffset(
update_map_value.at(pedal_index + 1).at(vel_index + 1) =
map_value.at(pedal_index + 1).at(vel_index + 1) + map_offset(3);

offset_covariance_value.at(pedal_index + 0).at(vel_index + 0) = std::sqrt(sigma(0));
offset_covariance_value.at(pedal_index + 1).at(vel_index + 1) = std::sqrt(sigma(1));
offset_covariance_value.at(pedal_index + 0).at(vel_index + 0) = std::sqrt(sigma(2));
offset_covariance_value.at(pedal_index + 1).at(vel_index + 1) = std::sqrt(sigma(3));

return true;
}

Expand Down Expand Up @@ -1342,6 +1420,43 @@ void AccelBrakeMapCalibrator::publishMap(
}
}

void AccelBrakeMapCalibrator::publishOffsetCovMap(
const std::vector<std::vector<double>> accel_map_value,
const std::vector<std::vector<double>> brake_map_value)
{
if (accel_map_value.at(0).size() != brake_map_value.at(0).size()) {
RCLCPP_ERROR_STREAM(
rclcpp::get_logger("accel_brake_map_calibrator"),
"Invalid map. The number of velocity index of accel map and brake map is different.");
return;
}
const double h = accel_map_value.size() + brake_map_value.size() -
1; // pedal (accel_map_value(0) and brake_map_value(0) is same.)
const double w = accel_map_value.at(0).size(); // velocity

// publish raw map
std_msgs::msg::Float32MultiArray float_map;

float_map.layout.dim.push_back(std_msgs::msg::MultiArrayDimension());
float_map.layout.dim.push_back(std_msgs::msg::MultiArrayDimension());
float_map.layout.dim[0].label = "height";
float_map.layout.dim[1].label = "width";
float_map.layout.dim[0].size = h;
float_map.layout.dim[1].size = w;
float_map.layout.dim[0].stride = h * w;
float_map.layout.dim[1].stride = w;
float_map.layout.data_offset = 0;
std::vector<float> vec(h * w, 0);
for (int i = 0; i < h; i++) {
for (int j = 0; j < w; j++) {
vec[i * w + j] =
static_cast<float>(getMapColumnFromUnifiedIndex(accel_map_value, brake_map_value, i).at(j));
}
}
float_map.data = vec;
offset_covariance_pub_->publish(float_map);
}

void AccelBrakeMapCalibrator::publishFloat32(const std::string publish_type, const double val)
{
tier4_debug_msgs::msg::Float32Stamped msg;
Expand Down

0 comments on commit bac20ee

Please sign in to comment.