| 137 | } |
| 138 | |
| 139 | void |
| 140 | Track3D::update( |
| 141 | double x, |
| 142 | double y, |
| 143 | double z, |
| 144 | double height, |
| 145 | double distance, |
| 146 | double data_assocation_score, |
| 147 | double confidence, |
| 148 | double min_confidence, |
| 149 | double min_confidence_detections, |
| 150 | open_ptrack::detection::DetectionSource* detection_source, |
| 151 | bool first_update) |
| 152 | { |
| 153 | if(std::isnan(x) or std::isnan(y) or std::isnan(z)) return; |
| 154 | |
| 155 | //Update Kalman filter |
| 156 | int difference; |
| 157 | double vx, vy, vz; |
| 158 | if (velocity_in_motion_term_) |
| 159 | { |
| 160 | ros::Duration d(2.0); //1.0 |
| 161 | ros::Duration d2(3.0); //2.0 |
| 162 | |
| 163 | double t = std::max(first_time_detected_.toSec(), (detection_source->getTime() - d).toSec()); |
| 164 | // std::cout << "t1: " << t << std::endl; |
| 165 | t = std::min(t, last_time_detected_.toSec()); |
| 166 | // std::cout << "t2: " << t << std::endl; |
| 167 | t = std::max(t, (detection_source->getTime() - d2).toSec()); |
| 168 | // std::cout << "t3: " << t << std::endl; |
| 169 | double dt = t - last_time_predicted_.toSec(); |
| 170 | // std::cout << "dt: " << dt << std::endl; |
| 171 | difference = int(round(dt / period_)); |
| 172 | // std::cout << "period: " << period_ << std::endl; |
| 173 | // std::cout << "difference: " << difference << std::endl; |
| 174 | int vIndex = (MAX_SIZE + last_time_predicted_index_ + difference) % MAX_SIZE; |
| 175 | // std::cout << vIndex << std::endl; |
| 176 | |
| 177 | if(difference != 0 |
| 178 | and not mahalanobis_map6d_[vIndex].x == 0 |
| 179 | and not mahalanobis_map6d_[vIndex].y == 0 |
| 180 | and not mahalanobis_map6d_[vIndex].z == 0) // prevent initial drift |
| 181 | { |
| 182 | vx = - (x - mahalanobis_map6d_[vIndex].x) / dt; |
| 183 | vy = - (y - mahalanobis_map6d_[vIndex].y) / dt; |
| 184 | vz = - (z - mahalanobis_map6d_[vIndex].z) / dt; |
| 185 | } |
| 186 | else |
| 187 | { |
| 188 | vx = mahalanobis_map6d_[vIndex].x; |
| 189 | vy = mahalanobis_map6d_[vIndex].y; |
| 190 | vz = mahalanobis_map6d_[vIndex].z; |
| 191 | } |
| 192 | // std::cout << "Mahalanobis: " << mahalanobis_map6d_[vIndex].x << "," << mahalanobis_map6d_[vIndex].y << "," << mahalanobis_map6d_[vIndex].z << std::endl; |
| 193 | } |
| 194 | // Update Kalman filter from the last time the track was visible: |
| 195 | int framesLost = int(round((detection_source->getTime() - last_time_detected_).toSec() / period_)) - 1; |
| 196 |
no test coverage detected