Mat LRTV::compute() { int max_iter = 30; for(int iter = 0; iter < max_iter; ++iter) { cout << "=====================" << endl; cout << "Iter = " << (iter + 1) << endl; sub_1(); sub_2(); sub_3(); // when to stop needs further consideration if( iter >= 5 && norm(U_, U_last_) / norm(U_last_) < 1.5e-3 ) { break; } cout << "relative error = " << 1.0* norm(U_, U_last_) / norm(U_last_) << endl; U_.copyTo(U_last_); cout << "PSNR = " << PSNR(I_, U_, mask_) << endl; cout << endl; } return U_; }
TEST(CanAcceptanceFilter, Basic_test) { uavcan::GlobalDataTypeRegistry::instance().reset(); uavcan::DefaultDataTypeRegistrator<uavcan::equipment::camera_gimbal::AngularCommand> _reg1; uavcan::DefaultDataTypeRegistrator<uavcan::equipment::air_data::Sideslip> _reg2; uavcan::DefaultDataTypeRegistrator<uavcan::equipment::air_data::TrueAirspeed> _reg3; uavcan::DefaultDataTypeRegistrator<uavcan::equipment::air_data::AngleOfAttack> _reg4; uavcan::DefaultDataTypeRegistrator<uavcan::equipment::ahrs::Solution> _reg5; uavcan::DefaultDataTypeRegistrator<uavcan::equipment::air_data::StaticPressure> _reg6; uavcan::DefaultDataTypeRegistrator<uavcan::protocol::file::BeginFirmwareUpdate> _reg7; SystemClockDriver clock_driver; CanDriverMock can_driver(1, clock_driver); TestNode node(can_driver, clock_driver, 24); uavcan::Subscriber<uavcan::equipment::camera_gimbal::AngularCommand, SubscriptionListener<uavcan::equipment::camera_gimbal::AngularCommand>::ExtendedBinder> sub_1(node); uavcan::Subscriber<uavcan::equipment::air_data::Sideslip, SubscriptionListener<uavcan::equipment::air_data::Sideslip>::ExtendedBinder> sub_2(node); uavcan::Subscriber<uavcan::equipment::air_data::TrueAirspeed, SubscriptionListener<uavcan::equipment::air_data::TrueAirspeed>::ExtendedBinder> sub_3(node); uavcan::Subscriber<uavcan::equipment::air_data::AngleOfAttack, SubscriptionListener<uavcan::equipment::air_data::AngleOfAttack>::ExtendedBinder> sub_4(node); uavcan::Subscriber<uavcan::equipment::ahrs::Solution, SubscriptionListener<uavcan::equipment::ahrs::Solution>::ExtendedBinder> sub_5(node); uavcan::Subscriber<uavcan::equipment::air_data::StaticPressure, SubscriptionListener<uavcan::equipment::air_data::StaticPressure>::ExtendedBinder> sub_6(node); uavcan::Subscriber<uavcan::equipment::air_data::StaticPressure, SubscriptionListener<uavcan::equipment::air_data::StaticPressure>::ExtendedBinder> sub_6_1(node); uavcan::ServiceServer<uavcan::protocol::file::BeginFirmwareUpdate> server(node); SubscriptionListener<uavcan::equipment::camera_gimbal::AngularCommand> listener_1; SubscriptionListener<uavcan::equipment::air_data::Sideslip> listener_2; SubscriptionListener<uavcan::equipment::air_data::TrueAirspeed> listener_3; SubscriptionListener<uavcan::equipment::air_data::AngleOfAttack> listener_4; SubscriptionListener<uavcan::equipment::ahrs::Solution> listener_5; SubscriptionListener<uavcan::equipment::air_data::StaticPressure> listener_6; sub_1.start(listener_1.bindExtended()); sub_2.start(listener_2.bindExtended()); sub_3.start(listener_3.bindExtended()); sub_4.start(listener_4.bindExtended()); sub_5.start(listener_5.bindExtended()); sub_6.start(listener_6.bindExtended()); sub_6_1.start(listener_6.bindExtended()); server.start(writeServiceServerCallback); std::cout << "Subscribers are initialized ..." << std::endl; uavcan::CanAcceptanceFilterConfigurator anon_test_configuration(node); int configure_filters_assert = anon_test_configuration.configureFilters(); if (configure_filters_assert == 0) { std::cout << "Filters are configured with anonymous configuration..." << std::endl; } const auto& configure_array = anon_test_configuration.getConfiguration(); uint32_t configure_array_size = configure_array.getSize(); ASSERT_EQ(configure_filters_assert, 0); ASSERT_EQ(configure_array_size, 4); for (uint16_t i = 0; i<configure_array_size; i++) { std::cout << "config.ID [" << i << "]= " << configure_array.getByIndex(i)->id << std::endl; std::cout << "config.MK [" << i << "]= " << configure_array.getByIndex(i)->mask << std::endl; } ASSERT_EQ(configure_array.getByIndex(0)->id, 0); ASSERT_EQ(configure_array.getByIndex(0)->mask, 255); ASSERT_EQ(configure_array.getByIndex(1)->id, 256000); ASSERT_EQ(configure_array.getByIndex(1)->mask, 16771968); ASSERT_EQ(configure_array.getByIndex(2)->id, 6272); ASSERT_EQ(configure_array.getByIndex(2)->mask, 32640); ASSERT_EQ(configure_array.getByIndex(3)->id, 262144); ASSERT_EQ(configure_array.getByIndex(3)->mask, 16771200); uavcan::CanAcceptanceFilterConfigurator no_anon_test_confiruration(node); configure_filters_assert = no_anon_test_confiruration.configureFilters (uavcan::CanAcceptanceFilterConfigurator::IgnoreAnonymousMessages); if (configure_filters_assert == 0) { std::cout << "Filters are configured without anonymous configuration..." << std::endl; } const auto& configure_array_2 = no_anon_test_confiruration.getConfiguration(); configure_array_size = configure_array_2.getSize(); ASSERT_EQ(configure_filters_assert, 0); ASSERT_EQ(configure_array_size, 4); for (uint16_t i = 0; i<configure_array_size; i++) { std::cout << "config.ID [" << i << "]= " << configure_array.getByIndex(i)->id << std::endl; std::cout << "config.MK [" << i << "]= " << configure_array.getByIndex(i)->mask << std::endl; } ASSERT_EQ(configure_array_2.getByIndex(0)->id, 6272); ASSERT_EQ(configure_array_2.getByIndex(0)->mask, 32640); ASSERT_EQ(configure_array_2.getByIndex(1)->id, 262144); ASSERT_EQ(configure_array_2.getByIndex(1)->mask, 16776320); ASSERT_EQ(configure_array_2.getByIndex(2)->id, 256000); ASSERT_EQ(configure_array_2.getByIndex(2)->mask, 16771968); ASSERT_EQ(configure_array_2.getByIndex(3)->id, 262144); ASSERT_EQ(configure_array_2.getByIndex(3)->mask, 16771968); }
TEST(CanAcceptanceFilter, Basic_test) { uavcan::GlobalDataTypeRegistry::instance().reset(); uavcan::DefaultDataTypeRegistrator<uavcan::equipment::camera_gimbal::AngularCommand> _reg1; uavcan::DefaultDataTypeRegistrator<uavcan::equipment::air_data::Sideslip> _reg2; uavcan::DefaultDataTypeRegistrator<uavcan::equipment::air_data::TrueAirspeed> _reg3; uavcan::DefaultDataTypeRegistrator<uavcan::equipment::air_data::AngleOfAttack> _reg4; uavcan::DefaultDataTypeRegistrator<uavcan::equipment::ahrs::AHRS> _reg5; uavcan::DefaultDataTypeRegistrator<uavcan::equipment::air_data::StaticPressure> _reg6; uavcan::DefaultDataTypeRegistrator<uavcan::protocol::file::BeginFirmwareUpdate> _reg7; SystemClockDriver clock_driver; CanDriverMock can_driver(1, clock_driver); TestNode node(can_driver, clock_driver, 24); uavcan::Subscriber<uavcan::equipment::camera_gimbal::AngularCommand, SubscriptionListener<uavcan::equipment::camera_gimbal::AngularCommand>::ExtendedBinder> sub_1(node); uavcan::Subscriber<uavcan::equipment::air_data::Sideslip, SubscriptionListener<uavcan::equipment::air_data::Sideslip>::ExtendedBinder> sub_2(node); uavcan::Subscriber<uavcan::equipment::air_data::TrueAirspeed, SubscriptionListener<uavcan::equipment::air_data::TrueAirspeed>::ExtendedBinder> sub_3(node); uavcan::Subscriber<uavcan::equipment::air_data::AngleOfAttack, SubscriptionListener<uavcan::equipment::air_data::AngleOfAttack>::ExtendedBinder> sub_4(node); uavcan::Subscriber<uavcan::equipment::ahrs::AHRS, SubscriptionListener<uavcan::equipment::ahrs::AHRS>::ExtendedBinder> sub_5(node); uavcan::Subscriber<uavcan::equipment::air_data::StaticPressure, SubscriptionListener<uavcan::equipment::air_data::StaticPressure>::ExtendedBinder> sub_6(node); uavcan::Subscriber<uavcan::equipment::air_data::StaticPressure, SubscriptionListener<uavcan::equipment::air_data::StaticPressure>::ExtendedBinder> sub_6_1(node); uavcan::ServiceServer<uavcan::protocol::file::BeginFirmwareUpdate> server(node); SubscriptionListener<uavcan::equipment::camera_gimbal::AngularCommand> listener_1; SubscriptionListener<uavcan::equipment::air_data::Sideslip> listener_2; SubscriptionListener<uavcan::equipment::air_data::TrueAirspeed> listener_3; SubscriptionListener<uavcan::equipment::air_data::AngleOfAttack> listener_4; SubscriptionListener<uavcan::equipment::ahrs::AHRS> listener_5; SubscriptionListener<uavcan::equipment::air_data::StaticPressure> listener_6; sub_1.start(listener_1.bindExtended()); sub_2.start(listener_2.bindExtended()); sub_3.start(listener_3.bindExtended()); sub_4.start(listener_4.bindExtended()); sub_5.start(listener_5.bindExtended()); sub_6.start(listener_6.bindExtended()); sub_6_1.start(listener_6.bindExtended()); server.start(writeServiceServerCallback); std::cout << "Subscribers are initialized ..." << std::endl; uavcan::CanAcceptanceFilterConfigurator test_configurator(node); int configure_filters_assert = test_configurator.configureFilters(); if (configure_filters_assert == 0) { std::cout << "Filters are configured ..." << std::endl; } const auto& configure_array = test_configurator.getConfiguration(); uint32_t configure_array_size = configure_array.getSize(); ASSERT_EQ(configure_filters_assert, 0); ASSERT_EQ(configure_array_size, 4); for (uint16_t i = 0; i<configure_array_size; i++) { std::cout << "config.ID [" << i << "]= " << configure_array.getByIndex(i)->id << std::endl; std::cout << "config.MK [" << i << "]= " << configure_array.getByIndex(i)->mask << std::endl; } ASSERT_EQ(configure_array.getByIndex(0)->id, 268435456); ASSERT_EQ(configure_array.getByIndex(0)->mask, 469762048); ASSERT_EQ(configure_array.getByIndex(1)->id, 363069440); ASSERT_EQ(configure_array.getByIndex(1)->mask, 536739840); ASSERT_EQ(configure_array.getByIndex(2)->id, 16777216); ASSERT_EQ(configure_array.getByIndex(2)->mask, 124452864); ASSERT_EQ(configure_array.getByIndex(3)->id, 18874368); ASSERT_EQ(configure_array.getByIndex(3)->mask, 133169152); }
int main() { return sub_3(10, 4, 2); }
static int _ggi_clip2d_3(ggi_visual *vis, int *_x0, int *_y0, int *_x1, int *_y1, int *clip_first, int *clip_last) { int first,last, code; int x0,y0,x1,y1; int x,y; unsigned dx[3], dy[3], tmp[3]; unsigned int absdx, absdy; int xmajor; int slope; int i; *clip_first = first = 0; *clip_last = last = 0; outcode(first,*_x0,*_y0); outcode(last,*_x1,*_y1); if ((first | last) == 0) { return 1; /* Trivially accepted! */ } if ((first & last) != 0) { return 0; /* Trivially rejected! */ } x0=*_x0; y0=*_y0; x1=*_x1; y1=*_y1; assign_int_3(dx, x1); assign_int_3(tmp, x0); sub_3(dx, tmp); assign_int_3(dy, y1); assign_int_3(tmp, y0); sub_3(dy, tmp); absdx = x0 < x1 ? x1 - x0 : x0 - x1; absdy = y0 < y1 ? y1 - y0 : y0 - y1; xmajor = absdx > absdy; slope = ((x1>=x0) && (y1>=y0)) || ((x1<x0) && (y1<y0)); for (i = 0; i < 4; i++) { code = first; if (first==0) code = last; if (code&OC_LEFT) { x = LIBGGI_GC(vis)->cliptl.x; if (xmajor) { /* y = *_y0 + FloorDiv(dy*(x - *_x0)*2 + dx, 2*dx); */ unsigned _x[3], res[3]; assign_int_3(_x, x); assign_int_3(tmp, *_x0); sub_3(_x, tmp); lshift_3(_x, 1); mul_3(_x, dy); add_3(_x, dx); assign_3(tmp, dx); lshift_3(tmp, 1); FloorDiv_3(res, _x, tmp); assign_int_3(tmp, *_y0); add_3(res, tmp); y = res[0]; } else if (slope) { /* y = *_y0 + CeilDiv(dy*((x - *_x0)*2 - 1), 2*dx); */ unsigned _x[3], res[3]; assign_int_3(_x, x); assign_int_3(tmp, *_x0); sub_3(_x, tmp); lshift_3(_x, 1); dec_3(_x); mul_3(_x, dy); assign_3(tmp, dx); lshift_3(tmp, 1); CeilDiv_3(res, _x, tmp); assign_int_3(tmp, *_y0); add_3(res, tmp); y = res[0]; } else { /* y = *_y0 + FloorDiv(dy*((x - *_x0)*2 - 1), 2*dx); */ unsigned _x[3], res[3]; assign_int_3(_x, x); assign_int_3(tmp, *_x0); sub_3(_x, tmp); lshift_3(_x, 1); dec_3(_x); mul_3(_x, dy); assign_3(tmp, dx); lshift_3(tmp, 1); FloorDiv_3(res, _x, tmp); assign_int_3(tmp, *_y0); add_3(res, tmp); y = res[0]; } } else if (code&OC_RIGHT) { x = LIBGGI_GC(vis)->clipbr.x - 1; if (xmajor) { /* y = *_y0 + FloorDiv(dy*(x - *_x0)*2 + dx, 2*dx); */ unsigned _x[3], res[3]; assign_int_3(_x, x); assign_int_3(tmp, *_x0); sub_3(_x, tmp); lshift_3(_x, 1); mul_3(_x, dy); add_3(_x, dx); assign_3(tmp, dx); lshift_3(tmp, 1); FloorDiv_3(res, _x, tmp); assign_int_3(tmp, *_y0); add_3(res, tmp); y = res[0]; } else if (slope) { /* y = *_y0 + CeilDiv(dy*((x - *_x0)*2 + 1), 2*dx)-1; */ unsigned _x[3], res[3]; assign_int_3(_x, x); assign_int_3(tmp, *_x0); sub_3(_x, tmp); lshift_3(_x, 1); inc_3(_x); mul_3(_x, dy); assign_3(tmp, dx); lshift_3(tmp, 1); CeilDiv_3(res, _x, tmp); dec_3(res); assign_int_3(tmp, *_y0); add_3(res, tmp); y = res[0]; } else { /* y = *_y0 + FloorDiv(dy*((x - *_x0)*2 + 1), 2*dx)+1; */ unsigned _x[3], res[3]; assign_int_3(_x, x); assign_int_3(tmp, *_x0); sub_3(_x, tmp); lshift_3(_x, 1); inc_3(_x); mul_3(_x, dy); assign_3(tmp, dx); lshift_3(tmp, 1); FloorDiv_3(res, _x, tmp); inc_3(res); assign_int_3(tmp, *_y0); add_3(res, tmp); y = res[0]; } } else if (code&OC_TOP) { y = LIBGGI_GC(vis)->cliptl.y; if (!xmajor) { /* x = *_x0 + FloorDiv(dx*(y - *_y0)*2 + dy, 2*dy); */ unsigned _y[3], res[3]; assign_int_3(_y, y); assign_int_3(tmp, *_y0); sub_3(_y, tmp); lshift_3(_y, 1); mul_3(_y, dx); add_3(_y, dy); assign_3(tmp, dy); lshift_3(tmp, 1); FloorDiv_3(res, _y, tmp); assign_int_3(tmp, *_x0); add_3(res, tmp); x = res[0]; } else if (slope) { /* x = *_x0 + CeilDiv(dx*((y - *_y0)*2 - 1), 2*dy); */ unsigned _y[3], res[3]; assign_int_3(_y, y); assign_int_3(tmp, *_y0); sub_3(_y, tmp); lshift_3(_y, 1); dec_3(_y); mul_3(_y, dx); assign_3(tmp, dy); lshift_3(tmp, 1); CeilDiv_3(res, _y, tmp); assign_int_3(tmp, *_x0); add_3(res, tmp); x = res[0]; } else { /* x = *_x0 + FloorDiv(dx*((y - *_y0)*2 - 1), 2*dy); */ unsigned _y[3], res[3]; assign_int_3(_y, y); assign_int_3(tmp, *_y0); sub_3(_y, tmp); lshift_3(_y, 1); dec_3(_y); mul_3(_y, dx); assign_3(tmp, dy); lshift_3(tmp, 1); FloorDiv_3(res, _y, tmp); assign_int_3(tmp, *_x0); add_3(res, tmp); x = res[0]; } } else { /* OC_BOTTOM */ LIB_ASSERT((code & OC_BOTTOM), "unknown outcode\n"); y = LIBGGI_GC(vis)->clipbr.y - 1; if (!xmajor) { /* x = *_x0 + FloorDiv(dx*(y - *_y0)*2 + dy, 2*dy); */ unsigned _y[3], res[3]; assign_int_3(_y, y); assign_int_3(tmp, *_y0); sub_3(_y, tmp); lshift_3(_y, 1); mul_3(_y, dx); add_3(_y, dy); assign_3(tmp, dy); lshift_3(tmp, 1); FloorDiv_3(res, _y, tmp); assign_int_3(tmp, *_x0); add_3(res, tmp); x = res[0]; } else if (slope) { /* x = *_x0 + CeilDiv(dx*((y - *_y0)*2 + 1), 2*dy)-1; */ unsigned _y[3], res[3]; assign_int_3(_y, y); assign_int_3(tmp, *_y0); sub_3(_y, tmp); lshift_3(_y, 1); inc_3(_y); mul_3(_y, dx); assign_3(tmp, dy); lshift_3(tmp, 1); CeilDiv_3(res, _y, tmp); dec_3(res); assign_int_3(tmp, *_x0); add_3(res, tmp); x = res[0]; } else { /* x = *_x0 + FloorDiv(dx*((y - *_y0)*2 + 1), 2*dy)+1; */ unsigned _y[3], res[3]; assign_int_3(_y, y); assign_int_3(tmp, *_y0); sub_3(_y, tmp); lshift_3(_y, 1); inc_3(_y); mul_3(_y, dx); assign_3(tmp, dy); lshift_3(tmp, 1); FloorDiv_3(res, _y, tmp); inc_3(res); assign_int_3(tmp, *_x0); add_3(res, tmp); x = res[0]; } } if (first!=0) { x0 = x; y0 = y; outcode(first,x0,y0); *clip_first = 1; } else { x1 = x; y1 = y; last = code; outcode(last,x1,y1); *clip_last = 1; } if ((first & last) != 0) { return 0; /* Trivially rejected! */ } if ((first | last) == 0) { *_x0=x0; *_y0=y0; *_x1=x1; *_y1=y1; return 1; /* Trivially accepted! */ } } return 0; /* Aieee! Failed to clip, clip whole line... */ }